3D transforms added

CMakeLists.txt

@@ -2,7 +2,8 @@ cmake_minimum_required(VERSION 2.8)
 project(whycon)
 
 # set some flags
-add_definitions(-std=c++11)
+#add_definitions(-std=c++11)
+add_definitions(-std=gnu++0x)
 SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_RELEASE} -Wall -O4 -march=native -Wfatal-errors")
 #SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS_RELEASE} -Wall -O0 -g -Wfatal-errors")
 set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_CURRENT_SOURCE_DIR}/cmake-configs")

README.md

@@ -17,8 +17,8 @@ The main contributors were [Matias Nitsche](https://scholar.google.co.uk/citatio
 
 | WhyCon version  | Application | Main features | Maintainer|
 | --------------- | ----------- | ------ | ----- |
-| [WhyCon-ROS](https://github.com/lrse/whycon) | general | 2D, 3D, ROS + lightweight | [Matias Nitsche](https://scholar.google.co.uk/citations?user=Z0hQoRUAAAAJ&hl=en&oi=ao) |
-| [WhyCon-Mini](http://labe.felk.cvut.cz/~tkrajnik/circle_detector/) | general | 2D, 3D, lightweight, autocalibration | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao)|
+| [WhyCon-ROS](../../) | general | 2D, 3D, ROS + lightweight | [Matias Nitsche](https://scholar.google.co.uk/citations?user=Z0hQoRUAAAAJ&hl=en&oi=ao) |
+| [WhyCon-orig](https://github.com/gestom/whycon-orig/) | general | 2D, 3D, lightweight, autocalibration | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao)|
 | [SwarmCon](https://github.com/gestom/CosPhi/tree/master/Localization) | μ-swarms | 2D, individual IDs, autocalibration | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao) |
 | [Caspa-WhyCon](http://robotics.fel.cvut.cz/faigl/caspa/) | UAVs | embedded, open HW-SW solution | [Jan Faigl](https://scholar.google.co.uk/citations?user=-finD_sAAAAJ&hl=en) |
 | [Social-card](https://github.com/strands-project/strands_social/tree/hydro-devel/social_card_reader) | HRI | orientation translated to commands  | [Tom Krajnik](http://scholar.google.co.uk/citations?user=Qv3nqgsAAAAJ&hl=en&oi=ao) |
@@ -56,7 +56,7 @@ For the latest development version (which should also work and may contain new e
 
 ### ROS
 
-The main directory should be placed inside a catkin workspace source-space (e.g.: ~catkin_ws/src).
+The main directory should be placed inside a catkin workspace source-space (e.g.: ~/catkin_ws/src).
 It can then be compiled simply by:
 
     catkin_make
@@ -85,7 +85,8 @@ Note the default CMake location is `/usr/local`, but you can redefine this by in
 
 ## Using WhyCon
 
-Please refer to the [wiki](https://github.com/lrse/whycon/wiki).
+For a step-by-step tutorial, please refer to the [wiki tutorial](../../wiki/Tutorial).
+For additional information, please check the [wiki home](../../wiki).
 
 ----
 

include/whycon/circle_detector.h

@@ -80,7 +80,7 @@ namespace cv {
           void write(cv::FileStorage& fs) const;
           void read(const cv::FileNode& node);
 
-          void draw(cv::Mat& image, const std::string& text = std::string(), cv::Vec3b color = cv::Vec3b(0,255,0), float thickness = 1) const;
+          void draw(cv::Mat& image, const std::string& text1 = std::string(), const std::string& text2 = std::string(), cv::Vec3b color = cv::Vec3b(0,255,0), float thickness = 1) const;
       };
 
       class Context {

include/whycon/localization_system.h

@@ -28,14 +28,18 @@ namespace cv {
       Pose get_pose(const CircleDetector::Circle& circle) const;
       const CircleDetector::Circle& get_circle(int id);
 
-      Pose get_transformed_pose(int id) const;
-      Pose get_transformed_pose(const CircleDetector::Circle& circle) const;
+      Pose get_transformed_pose_2d(int id) const;
+      Pose get_transformed_pose_3d(int id) const;
+      Pose get_transformed_pose_2d(const CircleDetector::Circle& circle) const;
+      Pose get_transformed_pose_3d(const CircleDetector::Circle& circle) const;
 
       static void load_matlab_calibration(const std::string& calib_file, cv::Mat& K, cv::Mat& dist_coeff);
       static void load_opencv_calibration(const std::string& calib_file, cv::Mat& K, cv::Mat& dist_coeff);
 
       CircleDetector::Circle origin_circles[4]; // center, X, Y
 
+      cv::Matx33f coordinates_transform_3d_rot;
+      cv::Vec3f coordinates_transform_3d_trn;
       cv::Matx33f coordinates_transform;      
       ManyCircleDetector detector;
 

src/lib/circle_detector.cpp

@@ -13,28 +13,28 @@ cv::CircleDetector::CircleDetector(int _width, int _height, Context* _context, f
 	context(_context)
 {
 	minSize = 10;
-  maxSize = 100*100; // TODO: test!
+	maxSize = 100*100; // TODO: test!
 	centerDistanceToleranceRatio = 0.1;
 	centerDistanceToleranceAbs = 5;
 	circularTolerance = 0.3;
 	ratioTolerance = 1.0;
-	
+
 	//initialization - fixed params
 	width = _width;
 	height = _height;
 	len = width*height;
 	siz = len*3;
-  diameterRatio = _diameter_ratio;
+	diameterRatio = _diameter_ratio;
 	float areaRatioInner_Outer = diameterRatio*diameterRatio;
 	outerAreaRatio = M_PI*(1.0-areaRatioInner_Outer)/4;
 	innerAreaRatio = M_PI/4.0;
 	areasRatio = (1.0-areaRatioInner_Outer)/areaRatioInner_Outer;
 
-  threshold = (3 * 256) / 2;
-  threshold_counter = 0;
+	threshold = (3 * 256) / 2;
+	threshold_counter = 0;
 
-  use_local_window = false;
-  local_window_multiplier = 2.5;
+	use_local_window = false;
+	local_window_multiplier = 2.5;
 }
 
 cv::CircleDetector::~CircleDetector()
@@ -74,21 +74,21 @@ inline int cv::CircleDetector::threshold_pixel(uchar* ptr)
 
 bool cv::CircleDetector::examineCircle(const cv::Mat& image, cv::CircleDetector::Circle& circle, int ii, float areaRatio, bool search_in_window)
 {
-  //int64_t ticks = cv::getTickCount();  
-  // get shorter names for elements in Context
+	//int64_t ticks = cv::getTickCount();  
+	// get shorter names for elements in Context
 	vector<int>& buffer = context->buffer;
-  vector<int>& queue = context->queue;
+	vector<int>& queue = context->queue;
 
-  int vx,vy;
+	int vx,vy;
 	queueOldStart = queueStart;
 	int position = 0;
 	int pos;	
 	bool result = false;
 	int type = buffer[ii];
 	int maxx,maxy,minx,miny;
-  int pixel_class;
+	int pixel_class;
 
-  WHYCON_DEBUG("examine (type " << type << ") at " << ii / width << "," << ii % width << " (numseg " << context->total_segments << ")");
+	WHYCON_DEBUG("examine (type " << type << ") at " << ii / width << "," << ii % width << " (numseg " << context->total_segments << ")");
 
 	int segment_id = context->total_segments++;
 	buffer[ii] = segment_id;
@@ -106,59 +106,59 @@ bool cv::CircleDetector::examineCircle(const cv::Mat& image, cv::CircleDetector:
 		position = queue[queueStart++];
 		//search neighbours
 
-    int position_x = position % width;
-    int position_y = position / width;
-
-    if ((search_in_window && position_x + 1 < min(local_window_x + local_window_width, width)) ||
-        (!search_in_window && position_x + 1 < width))
-    {
-      pos = position + 1;
-      pixel_class = buffer[pos];
-      if (is_unclassified(pixel_class)) {
-        uchar* ptr = &image.data[pos*3];
-        pixel_class = threshold_pixel(ptr);
-        if (pixel_class != type) buffer[pos] = pixel_class;
-      }
-      if (pixel_class == type) {
-        queue[queueEnd++] = pos;
-        maxx = max(maxx,pos%width);
-        buffer[pos] = segment_id;
-      }
-    }
-
-    if ((search_in_window && position_x - 1 >= local_window_x) ||
-        (!search_in_window && position_x - 1 >= 0))
-    {
-      pos = position-1;
-      pixel_class = buffer[pos];
-      if (is_unclassified(pixel_class)) {
-        uchar* ptr = &image.data[pos*3];
-        pixel_class = threshold_pixel(ptr);
-        if (pixel_class != type) buffer[pos] = pixel_class;
-      }
-      if (pixel_class == type) {
-        queue[queueEnd++] = pos;
-        minx = min(minx,pos%width);
-        buffer[pos] = segment_id;
-      }
-    }
+		int position_x = position % width;
+		int position_y = position / width;
 
-    if ((search_in_window && position_y - 1 >= local_window_y) ||
-        (!search_in_window && position_y - 1 >= 0))
-    {
-      pos = position-width;
-      pixel_class = buffer[pos];
-      if (is_unclassified(pixel_class)) {
-        uchar* ptr = &image.data[pos*3];
-        pixel_class = threshold_pixel(ptr);
-        if (pixel_class != type) buffer[pos] = pixel_class;
-      }
-      if (pixel_class == type) {
-        queue[queueEnd++] = pos;
-        miny = min(miny,pos/width);
-        buffer[pos] = segment_id;
-      }
-    }
+		if ((search_in_window && position_x + 1 < min(local_window_x + local_window_width, width)) ||
+				(!search_in_window && position_x + 1 < width))
+		{
+			pos = position + 1;
+			pixel_class = buffer[pos];
+			if (is_unclassified(pixel_class)) {
+				uchar* ptr = &image.data[pos*3];
+				pixel_class = threshold_pixel(ptr);
+				if (pixel_class != type) buffer[pos] = pixel_class;
+			}
+			if (pixel_class == type) {
+				queue[queueEnd++] = pos;
+				maxx = max(maxx,pos%width);
+				buffer[pos] = segment_id;
+			}
+		}
+
+		if ((search_in_window && position_x - 1 >= local_window_x) ||
+				(!search_in_window && position_x - 1 >= 0))
+		{
+			pos = position-1;
+			pixel_class = buffer[pos];
+			if (is_unclassified(pixel_class)) {
+				uchar* ptr = &image.data[pos*3];
+				pixel_class = threshold_pixel(ptr);
+				if (pixel_class != type) buffer[pos] = pixel_class;
+			}
+			if (pixel_class == type) {
+				queue[queueEnd++] = pos;
+				minx = min(minx,pos%width);
+				buffer[pos] = segment_id;
+			}
+		}
+
+		if ((search_in_window && position_y - 1 >= local_window_y) ||
+				(!search_in_window && position_y - 1 >= 0))
+		{
+			pos = position-width;
+			pixel_class = buffer[pos];
+			if (is_unclassified(pixel_class)) {
+				uchar* ptr = &image.data[pos*3];
+				pixel_class = threshold_pixel(ptr);
+				if (pixel_class != type) buffer[pos] = pixel_class;
+			}
+			if (pixel_class == type) {
+				queue[queueEnd++] = pos;
+				miny = min(miny,pos/width);
+				buffer[pos] = segment_id;
+			}
+		}
 
 		if ((search_in_window && position_y + 1 < min(local_window_y + local_window_height, height)) ||
 				(!search_in_window && position_y + 1 < height))
@@ -177,8 +177,8 @@ bool cv::CircleDetector::examineCircle(const cv::Mat& image, cv::CircleDetector:
 			}
 		}
 
-    //if (queueEnd-queueOldStart > maxSize) return false;
-  }
+		//if (queueEnd-queueOldStart > maxSize) return false;
+	}
 
 	//once the queue is empty, i.e. segment is complete, we compute its size 
 	circle.size = queueEnd-queueOldStart;
@@ -200,7 +200,7 @@ bool cv::CircleDetector::examineCircle(const cv::Mat& image, cv::CircleDetector:
 			//if its round, we compute yet another properties 
 			circle.round = true;
 
-      // TODO: mean computation could be delayed until the inner ring also satisfies above condition, right?
+			// TODO: mean computation could be delayed until the inner ring also satisfies above condition, right?
 			circle.mean = 0;
 			for (int p = queueOldStart;p<queueEnd;p++){
 				pos = queue[p];
@@ -213,8 +213,8 @@ bool cv::CircleDetector::examineCircle(const cv::Mat& image, cv::CircleDetector:
 	}
 	else WHYCON_DEBUG("not large enough (" << circle.size << "/" << minSize << ")");
 
-  //double delta = (double)(cv::getTickCount() - ticks) / cv::getTickFrequency();
-  //cout << "examineCircle: " << delta << " " << " fps: " << 1/delta << " pix: " << circle.size << " " << threshold << endl;
+	//double delta = (double)(cv::getTickCount() - ticks) / cv::getTickFrequency();
+	//cout << "examineCircle: " << delta << " " << " fps: " << 1/delta << " pix: " << circle.size << " " << threshold << endl;
 
 	return result;
 }
@@ -474,7 +474,7 @@ cv::CircleDetector::Circle::Circle(void)
   round = valid = false;
 }
 
-void cv::CircleDetector::Circle::draw(cv::Mat& image, const std::string& text, cv::Vec3b color, float thickness) const
+void cv::CircleDetector::Circle::draw(cv::Mat& image, const std::string& text1, const std::string& text2 , cv::Vec3b color, float thickness) const
 {
   for (float e = 0; e < 2 * M_PI; e += 0.05) {
     float fx = x + cos(e) * v0 * m0 * 2 + v1 * m1 * 2 * sin(e);
@@ -488,7 +488,8 @@ void cv::CircleDetector::Circle::draw(cv::Mat& image, const std::string& text, c
   float scale = image.size().width / 1800.0f;
   //float thickness = scale * 3.0;
   //if (thickness < 1) thickness = 1;
-  cv::putText(image, text.c_str(), cv::Point(x + 2 * m0, y + 2 * m1), CV_FONT_HERSHEY_SIMPLEX, scale, cv::Scalar(color), thickness, CV_AA);
+  cv::putText(image, text1.c_str(), cv::Point(x + 2 * m0, y + 2 * m1), CV_FONT_HERSHEY_SIMPLEX, scale, cv::Scalar(color), thickness, CV_AA);
+  cv::putText(image, text2.c_str(), cv::Point(x + 2 * m0, y + 2 * m1+32*scale), CV_FONT_HERSHEY_SIMPLEX, scale, cv::Scalar(color), thickness, CV_AA);
   cv::line(image, cv::Point(x + v0 * m0 * 2, y + v1 * m0 * 2), cv::Point(x - v0 * m0 * 2, y - v1 * m0 * 2), cv::Scalar(color), 1, 8);
   cv::line(image, cv::Point(x + v1 * m1 * 2, y - v0 * m1 * 2), cv::Point(x - v1 * m1 * 2, y + v0 * m1 * 2), cv::Scalar(color), 1, 8); 
 }