42 static const std::string
LOGNAME =
"shape_mask";
45 : transform_callback_(transform_callback), next_handle_(1), min_handle_(1)
63 boost::mutex::scoped_lock _(shapes_lock_);
64 transform_callback_ = transform_callback;
68 double scale,
double padding)
70 boost::mutex::scoped_lock _(shapes_lock_);
81 std::pair<std::set<SeeShape, SortBodies>::iterator,
bool> insert_op = bodies_.insert(ss);
82 if (!insert_op.second)
84 used_handles_[next_handle_] = insert_op.first;
90 const std::size_t sz = min_handle_ + bodies_.size() + 1;
91 for (std::size_t i = min_handle_; i < sz; ++i)
92 if (used_handles_.find(i) == used_handles_.end())
97 min_handle_ = next_handle_;
104 boost::mutex::scoped_lock _(shapes_lock_);
105 std::map<ShapeHandle, std::set<SeeShape, SortBodies>::iterator>::iterator it = used_handles_.find(handle);
106 if (it != used_handles_.end())
108 delete it->second->body;
109 bodies_.erase(it->second);
110 used_handles_.erase(it);
111 min_handle_ = handle;
118 const Eigen::Vector3d& ,
119 const double min_sensor_dist,
const double max_sensor_dist,
120 std::vector<int>& mask)
122 boost::mutex::scoped_lock _(shapes_lock_);
123 const unsigned int np = data_in.data.size() / data_in.point_step;
127 std::fill(mask.begin(), mask.end(), (
int)OUTSIDE);
130 Eigen::Isometry3d tmp;
131 bspheres_.resize(bodies_.size());
133 for (std::set<SeeShape>::const_iterator it = bodies_.begin(); it != bodies_.end(); ++it)
135 if (!transform_callback_(it->handle, tmp))
145 it->body->setPose(tmp);
146 it->body->computeBoundingSphere(bspheres_[j++]);
153 const double radius_squared =
bound.radius *
bound.radius;
163 for (
int i = 0; i < (int)np; ++i)
165 Eigen::Vector3d pt = Eigen::Vector3d(*(iter_x + i), *(iter_y + i), *(iter_z + i));
166 double d = pt.norm();
168 if (d < min_sensor_dist || d > max_sensor_dist)
170 else if ((
bound.center - pt).squaredNorm() < radius_squared)
171 for (std::set<SeeShape>::const_iterator it = bodies_.begin(); it != bodies_.end() && out == OUTSIDE; ++it)
172 if (it->body->containsPoint(pt))
181 boost::mutex::scoped_lock _(shapes_lock_);
184 for (std::set<SeeShape>::const_iterator it = bodies_.begin(); it != bodies_.end() && out == OUTSIDE; ++it)
185 if (it->body->containsPoint(pt))
192 return getMaskContainment(Eigen::Vector3d(
x,
y,
z));