23 #include "objpos_majority.h"
30 #include <core/threading/mutex_locker.h>
31 #include <blackboard/blackboard.h>
32 #include <logging/logger.h>
73 const std::string& own_id,
74 const std::string& foreign_id_pattern,
75 const std::string& output_id,
76 float self_confidence_radius)
78 blackboard_(blackboard),
80 output_id_(output_id),
81 self_confidence_radius_(self_confidence_radius)
88 foreign_id_pattern.c_str());
89 for (std::list<Opi*>::const_iterator it = input_ifs.begin();
90 it != input_ifs.end(); ++it) {
92 std::pair<OpiSet::iterator,bool> ret = input_ifs_.insert(opi);
94 blackboard->
close(opi);
97 if (own_if_ != NULL) {
98 std::pair<OpiSet::iterator,bool> ret = input_ifs_.insert(own_if_);
100 blackboard->
close(own_if_);
101 own_if_ = *ret.first;
107 if (iter != input_ifs_.end()) {
109 blackboard->
close(opi);
110 input_ifs_.erase(iter);
113 for (OpiSet::const_iterator it = input_ifs_.begin();
114 it != input_ifs_.end(); ++it) {
115 blackboard->
close(*it);
118 if (output_if_ != NULL) {
119 blackboard->
close(output_if_);
128 foreign_id_pattern.c_str());
139 for (OpiSet::const_iterator it = input_ifs_.begin();
140 it != input_ifs_.end(); ++it) {
141 blackboard_->
close(*it);
146 if (output_if_ != NULL) {
147 blackboard_->
close(output_if_);
154 const char *
id)
throw()
156 if (output_id_ ==
id) {
161 from_if = blackboard_->open_for_reading<
Opi>(id);
162 std::pair<OpiSet::iterator,bool> ret = input_ifs_.insert_locked(from_if);
164 blackboard_->close(from_if);
166 Opi* inserted_if = *ret.first;
167 if (own_if_ == NULL && own_id_ == std::string(inserted_if->
id())) {
168 own_if_ = inserted_if;
171 if (from_if != NULL) {
172 blackboard_->close(from_if);
179 WorldModelObjPosMajorityFuser::length(
float x,
float y,
float z)
181 return sqrt(x*x + y*y + z*z);
185 WorldModelObjPosMajorityFuser::rel_length(
const Opi* opi)
187 return length(opi->relative_x(), opi->relative_y(), opi->relative_z());
191 WorldModelObjPosMajorityFuser::world_object_dist(
const Opi* from,
const Opi* to)
193 return length(to->world_x() - from->world_x(),
194 to->world_y() - from->world_y(),
195 to->world_z() - from->world_z());
199 WorldModelObjPosMajorityFuser::same_contents(
const OpiBucket& left,
200 const OpiBucket& right)
202 if (left.size() != right.size()) {
206 std::set<OpiWrapper> rightSet(right.begin(), right.end());
207 for (OpiBucket::const_iterator it = left.begin();
208 it != left.end(); ++it) {
210 if (rightSet.find(opi) == rightSet.end()) {
220 if (own_if_ != NULL) {
224 if (own_if_ != NULL &&
229 rel_length(own_if_) <= self_confidence_radius_) ||
231 own_if_->
distance() <= self_confidence_radius_))) {
239 for (OpiSet::const_iterator it = input_ifs_.begin();
240 it != input_ifs_.end(); ++it) {
242 if (opi != own_if_) {
251 for (OpiSet::const_iterator it = input_ifs_.begin();
252 it != input_ifs_.end(); ++it) {
261 bucket.push_back(opi);
262 for (OpiSet::const_iterator jt = input_ifs_.begin();
263 jt != input_ifs_.end(); ++jt) {
264 Opi* candidate = *jt;
265 if (candidate != opi &&
267 world_object_dist(opi, candidate) <= GROUP_RADIUS) {
268 bucket.push_back(candidate);
271 buckets.push_back(bucket);
277 bool unambiguous =
false;
278 for (OpiBuckets::const_iterator it = buckets.begin();
279 it != buckets.end(); ++it) {
280 const OpiBucket& bucket = *it;
281 if (majority.size() <= bucket.size()) {
282 if (majority.size() < bucket.size()) {
286 unambiguous = unambiguous && same_contents(majority, bucket);
290 if (majority.size() > 0 && unambiguous) {
301 WorldModelObjPosMajorityFuser::check()
303 unsigned int base_flags = 0;
305 unsigned int object_type = 0;
306 bool object_type_warned =
false;
307 bool flags_read =
false;
309 for (OpiSet::const_iterator it = input_ifs_.begin();
310 it != input_ifs_.end(); ++it) {
312 if (!opi->has_writer()) {
315 if (!opi->is_valid()) {
318 if (object_type != 0 && opi->object_type() != object_type &&
319 !object_type_warned) {
320 logger_->
log_warn(
"WMObjPosAvgFus",
"Object types of input interfaces "
321 "for %s disagree, %s has %u, expected was %u",
322 output_id_.c_str(), opi->uid(), opi->object_type(),
324 object_type_warned =
true;
326 object_type = opi->object_type();
330 unsigned int iflags = opi->flags()
334 if (iflags != base_flags) {
335 logger_->
log_warn(
"WMObjPosAvgFus",
"Interface flags for %s "
336 "disagree. Exected %x, got %x", base_flags,
340 base_flags = opi->flags()
342 & (0xFFFFFFFF ^ Opi::FLAG_HAS_RELATIVE_CARTESIAN)
350 WorldModelObjPosMajorityFuser::copy_own_if()
363 WorldModelObjPosMajorityFuser::average(
const OpiBucket& input_ifs)
365 unsigned int flags = 0;
366 unsigned int world_num_inputs = 0;
367 unsigned int extent_num_inputs = 0;
368 unsigned int euler_num_inputs = 0;
369 unsigned int worldvel_num_inputs = 0;
370 unsigned int relcart_num_inputs = 0;
371 unsigned int relpolar_num_inputs = 0;
377 float bearing = 0.0f;
380 float world_x = 0.0f;
381 float world_y = 0.0f;
382 float world_z = 0.0f;
384 float relative_x = 0.0f;
385 float relative_y = 0.0f;
386 float relative_z = 0.0f;
388 float extent_x = 0.0f;
389 float extent_y = 0.0f;
390 float extent_z = 0.0f;
392 float world_x_velocity = 0.0f;
393 float world_y_velocity = 0.0f;
394 float world_z_velocity = 0.0f;
396 float relative_x_velocity = 0.0f;
397 float relative_y_velocity = 0.0f;
398 float relative_z_velocity = 0.0f;
402 int vishistory_min = 0;
403 int vishistory_max = 0;
404 bool have_world =
false, have_relative =
false;
406 for (OpiBucket::const_iterator it = input_ifs.begin();
407 it != input_ifs.end(); ++it) {
409 if (!opi->has_writer()) {
413 if (!opi->is_valid()) {
417 if (opi->is_visible()) {
422 world_x += opi->world_x();
423 world_y += opi->world_y();
424 world_z += opi->world_z();
425 world_num_inputs += 1;
429 pitch += opi->pitch();
432 euler_num_inputs += 1;
436 world_x_velocity += opi->world_x_velocity();
437 world_y_velocity += opi->world_y_velocity();
438 world_z_velocity += opi->world_z_velocity();
440 worldvel_num_inputs += 1;
445 have_relative =
true;
449 relative_x += opi->relative_x();
450 relative_y += opi->relative_y();
451 relative_z += opi->relative_z();
452 relative_x_velocity += opi->relative_x_velocity();
453 relative_y_velocity += opi->relative_y_velocity();
454 relative_z_velocity += opi->relative_z_velocity();
455 relcart_num_inputs += 1;
459 have_relative =
true;
463 distance += opi->distance();
464 bearing += opi->bearing();
465 slope += opi->slope();
466 relpolar_num_inputs += 1;
470 extent_x += opi->extent_x();
471 extent_y += opi->extent_y();
472 extent_z += opi->extent_z();
474 extent_num_inputs += 1;
477 if (opi->visibility_history() > vishistory_max) {
478 vishistory_max = opi->visibility_history();
481 if (opi->visibility_history() < vishistory_min) {
482 vishistory_min = opi->visibility_history();
487 if (world_num_inputs > 0) {
488 output_if_->
set_world_x(world_x / world_num_inputs);
489 output_if_->
set_world_y(world_y / world_num_inputs);
490 output_if_->
set_world_z(world_z / world_num_inputs);
492 if (euler_num_inputs > 0) {
493 output_if_->
set_roll(roll / euler_num_inputs);
494 output_if_->
set_pitch(pitch / euler_num_inputs);
495 output_if_->
set_yaw(yaw / euler_num_inputs);
497 if (worldvel_num_inputs > 0) {
503 if (extent_num_inputs > 0) {
508 if (relcart_num_inputs > 0) {
516 if (relpolar_num_inputs > 0) {
517 output_if_->
set_distance(distance / (
float)relpolar_num_inputs);
518 output_if_->
set_bearing(bearing / (
float)relpolar_num_inputs);
519 output_if_->
set_slope(slope / (
float)relpolar_num_inputs);
522 visible = have_world || have_relative;