1 #include <iostream>
2 #include <algorithm>
3 #include "volm_desc_ex_2d_matcher.h"
4 #ifdef _MSC_VER
5 # include "vcl_msvc_warnings.h"
6 #endif
7 //
8 // \file
9
score(volm_desc_sptr const & a,volm_desc_sptr const & b)10 float volm_desc_ex_2d_matcher::score(volm_desc_sptr const& a, volm_desc_sptr const& b)
11 {
12 unsigned nbins = a->nbins();
13 if (b->nbins() != nbins)
14 return 0.0f;
15 float score = 0.0f;
16 for (unsigned idx = 0; idx < nbins; idx++) {
17 float intersec = (float)std::min(a->count(idx), b->count(idx));
18 score += intersec*weights_hist_[idx];
19 }
20 return score;
21 /*return a->similarity(b);*/
22 }
23
24 // find the object weight value from object name (return 0 if not found)
find_wgt_value(std::string const & name)25 float volm_desc_ex_2d_matcher::find_wgt_value(std::string const& name)
26 {
27 for (auto & weight : weights_)
28 if ( weight.w_name_.compare(name) == 0)
29 return weight.w_obj_;
30 return 0.0f;
31 }
32
create_query_desc()33 volm_desc_sptr volm_desc_ex_2d_matcher::create_query_desc()
34 {
35 if (radius_.empty())
36 radius_.push_back(1.0);
37 // sort the radius to ensure the bin order
38 std::sort(radius_.begin(), radius_.end());
39 unsigned ndists = (unsigned)radius_.size() + 1;
40
41 double largest_rad = radius_[radius_.size()-1]; // this is in meters
42
43 // create the ex_land_only descriptor
44 auto* desc = new volm_desc_ex_land_only(ndists, nlands_, radius_);
45
46 // re-define the weight parameter to satisfy current radius setting (note this destroy previous weight ratio...)
47 float extra_wgt = 0.0f;
48 unsigned excluded_obj = 0;
49 if (!dms_->sky().empty())
50 extra_wgt += this->find_wgt_value("sky");
51 if (!dms_->scene_regions().empty()) {
52 std::vector<depth_map_region_sptr> objs = dms_->scene_regions();
53 for (auto & obj : objs) {
54 if (obj->min_depth() > largest_rad || !obj->active()) {
55 extra_wgt += this->find_wgt_value(obj->name());
56 excluded_obj++;
57 }
58 }
59 }
60 unsigned n_label = 0;
61 if (!dms_->ground_plane().empty())
62 n_label++;
63 n_label += dms_->scene_regions().size() - excluded_obj;
64
65 float wgt_inc = extra_wgt / n_label;
66 if (!dms_->ground_plane().empty()) {
67 for (auto & weight : weights_)
68 if ( weight.w_typ_.compare("ground_plane") == 0) {
69 weight.w_obj_ += wgt_inc;
70 break;
71 }
72 }
73 if (!dms_->scene_regions().empty()) {
74 std::vector<depth_map_region_sptr> objs = dms_->scene_regions();
75 for (auto & obj : objs) {
76 if (obj->min_depth() > largest_rad)
77 continue;
78 for (auto & weight : weights_)
79 if ( weight.w_name_.compare(obj->name()) == 0) {
80 weight.w_obj_ += wgt_inc;
81 break;
82 }
83 }
84 }
85
86 // loop over the depth map scene to create query histogram, along with re-defined weight
87 std::vector<double> radius = desc->radius();
88 unsigned nbins = desc->nbins();
89 weights_hist_.resize(nbins);
90
91 // ground
92 if (!dms_->ground_plane().empty()) {
93 std::vector<unsigned> grd_bin_id;
94 std::vector<depth_map_region_sptr> grd = dms_->ground_plane();
95 for (auto & g_idx : grd) {
96 if (!g_idx->active() || g_idx->min_depth() > largest_rad)
97 continue;
98 double min_depth = g_idx->min_depth();
99 double max_depth = g_idx->max_depth();
100 unsigned char land_id = g_idx->land_id();
101 double dist = min_depth;
102 if (dist > max_depth)
103 continue;
104 grd_bin_id.push_back(desc->bin_index(dist, land_id));
105 desc->set_count(dist, land_id, (unsigned char)1);
106 for (double radiu : radius) {
107 //dist = min_depth + radius[i]+1;
108 dist = radiu;
109 if (dist < min_depth)
110 continue;
111 if (dist < max_depth && dist < largest_rad) {
112 grd_bin_id.push_back(desc->bin_index(dist, land_id));
113 desc->set_count(dist, land_id, (unsigned char)1);
114 }
115 }
116 }
117 unsigned n_grd_bin = grd_bin_id.size();
118 float w_grd_total, w_grd;
119 w_grd_total = this->find_wgt_value("ground_plane");
120 w_grd = w_grd_total/n_grd_bin;
121 for (unsigned int & vit : grd_bin_id)
122 weights_hist_[vit] += w_grd;
123 }
124
125 // objects
126 if (!dms_->scene_regions().empty()) {
127 std::vector<depth_map_region_sptr> objs = dms_->scene_regions();
128 for (auto & obj : objs) {
129 if ( !obj->active() || obj->min_depth() > largest_rad)
130 continue;
131 std::vector<unsigned> obj_bin_id;
132 double min_d = obj->min_depth();
133 double max_d = obj->max_depth();
134 unsigned char land_id = obj->land_id();
135 float wgt_obj = this->find_wgt_value(obj->name());
136 double dist = min_d;
137 if (dist > max_d)
138 continue;
139 obj_bin_id.push_back(desc->bin_index(dist, land_id));
140 desc->set_count(dist, land_id, (unsigned char)1);
141 for (double radiu : radius) {
142 //dist = min_d + radius[i] + 1;
143 dist = radiu;
144 if (dist <= min_d)
145 continue;
146 if (dist < max_d && dist < largest_rad) {
147 obj_bin_id.push_back(desc->bin_index(dist, land_id));
148 desc->set_count(dist, land_id, (unsigned char)1);
149 }
150 }
151 unsigned n_obj_bin = obj_bin_id.size();
152 float w_obj = wgt_obj / n_obj_bin;
153 for (unsigned int & vit : obj_bin_id)
154 weights_hist_[vit] += w_obj;
155 }
156 }
157 volm_desc_sptr query(desc);
158 #if 0
159 std::cout << "generated weight histogram:" << std::endl;
160 for (std::vector<float>::iterator vit = weights_hist_.begin(); vit != weights_hist_.end(); ++vit)
161 std::cout << *vit << ' ';
162 std::cout << std::endl;
163 #endif
164
165 return query;
166 }
167
check_threshold(volm_desc_sptr const & query,float & thres_value)168 bool volm_desc_ex_2d_matcher::check_threshold(volm_desc_sptr const& query, float& thres_value)
169 {
170 unsigned num_valid_bin = query->get_area();
171 if (thres_value < 1.0f/num_valid_bin)
172 thres_value = 1.0f/num_valid_bin;
173 return true;
174 }
175