Shape_detect:

	Add method arcs_compare() for comparing of two detected arcs vectors.
	Add documentation.

diff --git a/src/hokuyo/shape-detect/shape_detect.cc b/src/hokuyo/shape-detect/shape_detect.cc
index 2090ee569bbd4495ed71cfdd70745ceb4a0aa2c7..34be3c70351b54db57042f78762132ae2de5fe28 100644 (file)
--- a/src/hokuyo/shape-detect/shape_detect.cc
+++ b/src/hokuyo/shape-detect/shape_detect.cc
@@ -43,6 +43,32 @@ inline float Shape_detect::point_distance(Shape_detect::Point a, Shape_detect::P
        return sqrt((a.x-b.x)*(a.x-b.x)+(a.y-b.y)*(a.y-b.y));
 }
 
+std::vector<Shape_detect::Arc> Shape_detect::arcs_compare(std::vector<Shape_detect::Arc> &first, std::vector<Shape_detect::Arc> &second, int eps)
+{
+       std::vector<Shape_detect::Arc> result;
+
+       if (first.size() < 1 && second.size() < 1) {
+               return result;
+       }
+       if (first.size() < 1 && second.size() > 0) {
+               return second;
+       }
+       if (first.size() > 0 && second.size() < 1) {
+               return first;
+       }
+
+       for (unsigned int i = 0; i < first.size(); i++) {
+               for (unsigned int j = 0; j < second.size(); j++) {
+                       if (point_distance(first[i].center, second[j].center) < eps) {
+                               result.push_back(first[i]);
+                               break;
+                       }
+               }
+       }
+
+       return result;
+}
+
 void Shape_detect::bresenham_circle(const float &radius, const float &scale, std::vector<Shape_detect::Point> &circle)
 {
        int d;
@@ -365,6 +391,7 @@ std::vector<Shape_detect::Point> &Shape_detect::getCartes()
 
 void Shape_detect::prepare(const unsigned short laser_scan[])
 {
+       cartes.clear();
        polar_to_cartes(laser_scan);
 }
 

diff --git a/src/hokuyo/shape-detect/shape_detect.h b/src/hokuyo/shape-detect/shape_detect.h
index 422af40e8b9122dc2fd5a3e105a5e804612e887b..5988e6489523a14361d33f6be7a40be53f48716f 100644 (file)
--- a/src/hokuyo/shape-detect/shape_detect.h
+++ b/src/hokuyo/shape-detect/shape_detect.h
@@ -167,7 +167,10 @@ class Shape_detect
                 */
                void prepare(const unsigned short laser_scan[]);
 
-               /** Returns laser_scan data set by prepare converted to cartesian coordinates */
+               /** 
+                * Returns laser_scan data set by prepare converted to cartesian coordinates
+                * @ingroup shapedet
+                */
                std::vector<Point> &getCartes();
                
                
@@ -187,6 +190,16 @@ class Shape_detect
                 */
                void arc_detect(std::vector<Arc> &arcs);
 
+               /**
+                * Is uset for comparing of two detected arcs vectors.
+                * @param &first is vector for comparing.
+                * @param &second is vector for comparing.
+                * @param eps is pertubation measured center of arc.
+                * @return vector equality center of arc.
+                * @ingroup shapedet
+                */
+               std::vector<Arc> arcs_compare(std::vector<Arc> &first, std::vector<Arc> &second, int eps);
+
        private:
                /**
                 * The minimal number of points which can create segment line.