Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037 #include <moveit/point_containment_filter/shape_mask.h>
00038 #include <geometric_shapes/body_operations.h>
00039 #include <ros/console.h>
00040
00041 point_containment_filter::ShapeMask::ShapeMask(const TransformCallback& transform_callback) :
00042 transform_callback_(transform_callback),
00043 next_handle_ (1),
00044 min_handle_ (1)
00045 {
00046 }
00047
00048 point_containment_filter::ShapeMask::~ShapeMask()
00049 {
00050 freeMemory();
00051 }
00052
00053 void point_containment_filter::ShapeMask::freeMemory()
00054 {
00055 for (std::set<SeeShape>::const_iterator it = bodies_.begin() ; it != bodies_.end() ; ++it)
00056 delete it->body;
00057 bodies_.clear();
00058 }
00059
00060 void point_containment_filter::ShapeMask::setTransformCallback(const TransformCallback& transform_callback)
00061 {
00062 boost::mutex::scoped_lock _(shapes_lock_);
00063 transform_callback_ = transform_callback;
00064 }
00065
00066 point_containment_filter::ShapeHandle point_containment_filter::ShapeMask::addShape(const shapes::ShapeConstPtr &shape, double scale, double padding)
00067 {
00068 boost::mutex::scoped_lock _(shapes_lock_);
00069 SeeShape ss;
00070 ss.body = bodies::createBodyFromShape(shape.get());
00071 if (ss.body)
00072 {
00073 ss.body->setScale(scale);
00074 ss.body->setPadding(padding);
00075 ss.volume = ss.body->computeVolume();
00076 ss.handle = next_handle_;
00077 std::pair<std::set<SeeShape, SortBodies>::iterator, bool> insert_op = bodies_.insert(ss);
00078 if (!insert_op.second)
00079 ROS_ERROR("Internal error in management of bodies in ShapeMask. This is a serious error.");
00080 used_handles_[next_handle_] = insert_op.first;
00081 }
00082 else
00083 return 0;
00084
00085 ShapeHandle ret = next_handle_;
00086 const std::size_t sz = min_handle_ + bodies_.size() + 1;
00087 for (std::size_t i = min_handle_ ; i < sz ; ++i)
00088 if (used_handles_.find(i) == used_handles_.end())
00089 {
00090 next_handle_ = i;
00091 break;
00092 }
00093 min_handle_ = next_handle_;
00094
00095 return ret;
00096 }
00097
00098 void point_containment_filter::ShapeMask::removeShape(ShapeHandle handle)
00099 {
00100 boost::mutex::scoped_lock _(shapes_lock_);
00101 std::map<ShapeHandle, std::set<SeeShape, SortBodies>::iterator>::iterator it = used_handles_.find(handle);
00102 if (it != used_handles_.end())
00103 {
00104 delete it->second->body;
00105 bodies_.erase(it->second);
00106 used_handles_.erase(it);
00107 min_handle_ = handle;
00108 }
00109 else
00110 ROS_ERROR("Unable to remove shape handle %u", handle);
00111 }
00112
00113 void point_containment_filter::ShapeMask::maskContainment(const pcl::PointCloud<pcl::PointXYZ>& data_in,
00114 const Eigen::Vector3d &sensor_origin,
00115 const double min_sensor_dist, const double max_sensor_dist,
00116 std::vector<int> &mask)
00117 {
00118 boost::mutex::scoped_lock _(shapes_lock_);
00119 mask.resize(data_in.points.size());
00120 if (bodies_.empty())
00121 std::fill(mask.begin(), mask.end(), (int)OUTSIDE);
00122 else
00123 {
00124 Eigen::Affine3d tmp;
00125 bspheres_.resize(bodies_.size());
00126 std::size_t j = 0;
00127 for (std::set<SeeShape>::const_iterator it = bodies_.begin() ; it != bodies_.end() ; ++it)
00128 {
00129 if (transform_callback_(it->handle, tmp))
00130 {
00131 it->body->setPose(tmp);
00132 it->body->computeBoundingSphere(bspheres_[j++]);
00133 }
00134 }
00135
00136 const unsigned int np = data_in.points.size();
00137
00138
00139 bodies::BoundingSphere bound;
00140 bodies::mergeBoundingSpheres(bspheres_, bound);
00141 const double radiusSquared = bound.radius * bound.radius;
00142
00143
00144 #pragma omp parallel for schedule(dynamic)
00145 for (int i = 0 ; i < (int)np ; ++i)
00146 {
00147 Eigen::Vector3d pt = Eigen::Vector3d(data_in.points[i].x, data_in.points[i].y, data_in.points[i].z);
00148 double d = pt.norm();
00149 int out = OUTSIDE;
00150 if (d < min_sensor_dist || d > max_sensor_dist)
00151 out = CLIP;
00152 else
00153 if ((bound.center - pt).squaredNorm() < radiusSquared)
00154 for (std::set<SeeShape>::const_iterator it = bodies_.begin() ; it != bodies_.end() && out == OUTSIDE ; ++it)
00155 if (it->body->containsPoint(pt))
00156 out = INSIDE;
00157 mask[i] = out;
00158 }
00159 }
00160 }
00161
00162 int point_containment_filter::ShapeMask::getMaskContainment(const Eigen::Vector3d &pt) const
00163 {
00164 boost::mutex::scoped_lock _(shapes_lock_);
00165
00166 int out = OUTSIDE;
00167 for (std::set<SeeShape>::const_iterator it = bodies_.begin() ; it != bodies_.end() && out == OUTSIDE ; ++it)
00168 if (it->body->containsPoint(pt))
00169 out = INSIDE;
00170 return out;
00171 }
00172
00173 int point_containment_filter::ShapeMask::getMaskContainment(double x, double y, double z) const
00174 {
00175 return getMaskContainment(Eigen::Vector3d(x, y, z));
00176 }