more calibration stuff

include/whycon/circle_detector.h

@@ -40,7 +40,6 @@ namespace whycon {
       bool examineCircle(const cv::Mat& image, Circle& circle, int ii, float areaRatio, bool search_in_window);
       void cover_last_detected(cv::Mat& image);
 
-      void improveEllipse(const cv::Mat& image, Circle& c);
       int get_threshold(void) const;
 
     private:
@@ -86,6 +85,8 @@ namespace whycon {
           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;
+
+          Circle improveEllipse(const cv::Mat& image) const;
       };
 
       class Context {

launch/whycon.launch

@@ -6,8 +6,9 @@
     <node pkg="image_proc" type="image_proc" name="image_proc"/>
   </group>
 
-  <node name="whycon" type="whycon" pkg="whycon">
+  <node name="whycon" type="whycon" pkg="whycon" output="screen">
     <param name="targets" value="$(arg targets)"/>
+    <param name="name" value="$(arg name)"/>
   </node>
 
   <node name="transformer" type="transformer" pkg="whycon" output="screen"/>

src/lib/circle_detector.cpp

@@ -384,7 +384,8 @@ whycon::CircleDetector::Circle whycon::CircleDetector::detect(const cv::Mat& ima
                 /*inner_id = numSegments; outer_id = numSegments - 1;*/
                 threshold = (outer.mean + inner.mean) / 2; // use a new threshold estimate based on current detection
                 //cout << "threshold set to average: " << threshold << endl;
-
+
+#if 1
                 //pixel leakage correction
 								float r = diameter_ratio * diameter_ratio;
 								float m0o = sqrt(f0);
@@ -396,7 +397,10 @@ whycon::CircleDetector::Circle whycon::CircleDetector::detect(const cv::Mat& ima
 								float b = -(m0i+m1i)-(m0o+m1o)*r;
 								float c = (m0i*m1i)-(m0o*m1o)*r;
 							 	float t = (-b-sqrt(b*b-4*a*c))/(2*a);
-								m0i-=t;m1i-=t;m0o+=t;m1o+=t;
+								//m0i-=t;m1i-=t;m0o+=t;m1o+=t;
+#else
+								float t = 0;
+#endif
 
 								inner.m0 = sqrt(f0)+t;
 								inner.m1 = sqrt(f1)+t;
@@ -577,3 +581,57 @@ void whycon::CircleDetector::Context::debug_buffer(const cv::Mat& image, cv::Mat
     }
   }
 }
+
+whycon::CircleDetector::Circle whycon::CircleDetector::Circle::improveEllipse(const cv::Mat& image) const
+{
+	Circle new_circle = *this;
+
+	cv::Mat subimg;
+	int delta = 10;
+	cout << image.rows << " x " << image.cols << endl;
+	cv::Range row_range(max(0, miny - delta), min(maxy + delta, image.rows));
+	cv::Range col_range(max(0, minx - delta), min(maxx + delta, image.cols));
+	cout << row_range.start << " " << row_range.end << " " << col_range.start << " " << col_range.end << endl;
+	image(row_range, col_range).copyTo(subimg);
+	cv::Mat cannified;
+	cv::Canny(subimg, cannified, 4000, 8000, 5, true);
+
+	/*cv::namedWindow("bleh");
+	cv::imshow("bleh", subimg);
+	cv::waitKey();*/
+
+	std::vector< std::vector<cv::Point> > contours;
+	cv::findContours(cannified, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
+	if (contours.empty() || contours[0].size() < 5) return new_circle;
+
+	cv::Mat contour_img;
+	subimg.copyTo(contour_img);
+	cv::drawContours(contour_img, contours, 0, cv::Scalar(255,0,255), 1);
+
+	/*cv::namedWindow("bleh2");
+	cv::imshow("bleh2", contour_img);
+	cv::waitKey();*/
+
+
+	cv::RotatedRect rect = cv::fitEllipse(contours[0]);
+	cout << "old: " << x << " " << y << " " << m0 << " " << m1 << " " << v0 << " " << v1 << endl;
+	new_circle.y = rect.center.x + col_range.start; // x/y appear inverted
+	new_circle.x = rect.center.y + row_range.start;
+	/*float max_size = max(rect.size.width, rect.size.height);
+	float min_size = min(rect.size.width, rect.size.height);*/
+	new_circle.m0 = rect.size.width * 0.25;
+	new_circle.m1 = rect.size.height * 0.25;
+	new_circle.v0 = cos(rect.angle / 180.0 * M_PI);
+	new_circle.v1 = sin(rect.angle / 180.0 * M_PI);
+	cout << "new: " << new_circle.x << " " << new_circle.y << " " << new_circle.m0 << " " << new_circle.m1 << " " << new_circle.v0 << " " << new_circle.v1 << endl;
+
+	/*cv::Mat ellipse_img;
+	image(row_range, col_range).copyTo(subimg);
+	subimg.copyTo(ellipse_img);
+	cv::ellipse(ellipse_img, rect, cv::Scalar(255,0,255));
+	cv::namedWindow("bleh3");
+	cv::imshow("bleh3", ellipse_img);
+	cv::waitKey();*/
+
+	return new_circle;
+}

src/lib/localization_system.cpp

@@ -106,6 +106,10 @@ whycon::LocalizationSystem::Pose whycon::LocalizationSystem::get_pose(const whyc
 	int S3 = (result.pos(2) * z < 0 ? -1 : 1);
 	result.pos *= S3 * z;
 
+	/*float dist = sqrt(L1 * L1 * L1) * circle_diameter * 0.5;
+	std::cout << "d1 " << dist << " " << cv::norm(result.pos) << std::endl;*/
+
+
 	WHYCON_DEBUG("ellipse center: " << x << "," << y << " " << " computed position: " << result.pos << " " << result.pos / result.pos(2));
 
 	// rotation
@@ -116,6 +120,15 @@ whycon::LocalizationSystem::Pose whycon::LocalizationSystem::get_pose(const whyc
 	result.rot(2) = 0; /* not recoverable */
 	/* TODO: to be checked */
 
+	/*cv::Matx33d data_inv;
+	cv::invert(data, data_inv);
+	cv::Matx31d projection = data_inv * normal_mat.t();
+	std::cout << "det " << cv::determinant(data_inv) << std::endl;
+	//cv::Vec3f new_center = cv::normalize(cv::Vec3f(projection(0), projection(1), projection(2))) * dist;
+	cv::Vec3f new_center = cv::Vec3f(projection(0) / projection(2), projection(1) / projection(2), 1) * dist;
+	std::cout << "center: " << new_center(0) << "," << new_center(1) << "," << new_center(2) << " vs " << result.pos << std::endl;
+	std::cout << "normalized: " << cv::normalize(new_center) << " " << cv::normalize(result.pos) << std::endl;*/
+
   return result;
 }
 

src/ros/set_axis.cpp

@@ -30,6 +30,11 @@ whycon::AxisSetter::AxisSetter(ros::NodeHandle &n)
 
 	if (!n.getParam("xscale", xscale) || !n.getParam("yscale", yscale)) throw std::runtime_error("Please specify xscale and yscale");
 
+	// axis should be specified in same order as detect_square
+	if (n.getParam("axis_order", axis_order)) {
+		if (axis_order.size() != 4) throw std::runtime_error("Exactly four indices are needed for specifying axis");
+	}
+
 	poses_sub = n.subscribe("/whycon/poses", 1, &AxisSetter::on_poses, this);
 	image_sub = n.subscribe("/camera/image_rect_color", 1, &AxisSetter::on_image, this);
 	image_pub = n.advertise<sensor_msgs::Image>("image", 1);
@@ -38,12 +43,8 @@ whycon::AxisSetter::AxisSetter(ros::NodeHandle &n)
 /**
  * Return four tf::Point's ordered as (0,0),(0,1),(1,0),(1,1)
  */
-void whycon::AxisSetter::detect_square(const std::vector<geometry_msgs::Pose>& msg_poses, std::vector<tf::Point>& out_points)
+void whycon::AxisSetter::detect_square(std::vector<tf::Point>& points)
 {
-	std::vector<tf::Point> points(4);
-	for (int i = 0; i < 4; i++)
-		tf::pointMsgToTF(msg_poses[i].position, points[i]);
-
 	tf::Point vectors[3];
 	for (int i = 0; i < 3; i++) {
 		vectors[i] = points[i + 1] - points[0];
@@ -63,15 +64,26 @@ void whycon::AxisSetter::detect_square(const std::vector<geometry_msgs::Pose>& m
   int xy_i = 0;
   for (int i = 1; i <= 3; i++) if (i != axis1_i && i != axis2_i) { xy_i = i; break; }
 
-	out_points.resize(4);
-	out_points[0] = points[0];
-	out_points[1] = points[axis1_i];
-	out_points[2] = points[axis2_i];
-	out_points[3] = points[xy_i];
+	std::vector<tf::Point> points_original(points);
+	points.resize(4);
+	points[0] = points_original[0];
+	points[1] = points_original[axis1_i];
+	points[2] = points_original[axis2_i];
+	points[3] = points_original[xy_i];
 
 	ROS_INFO_STREAM("Axis: (0,0) -> 0, (1,0) -> " << axis1_i << ", (0,1) -> " << axis2_i << ", (1,1) -> " << xy_i);
 }
 
+void whycon::AxisSetter::build_square(std::vector<tf::Point>& points)
+{
+	std::vector<tf::Point> points_original(points);
+	points.resize(4);
+	for (int i = 0; i < 4; i++)
+		points[i] = points_original[axis_order[i]];
+
+	ROS_INFO_STREAM("Axis: (0,0) -> " << axis_order[0] << ", (1,0) -> " << axis_order[1] << ", (0,1) -> " << axis_order[2] << ", (1,1) -> " << axis_order[3]);
+}
+
 tf::Matrix3x3 whycon::AxisSetter::compute_projection(const std::vector<tf::Point>& points, float xscale, float yscale)
 {
 	/* TODO: use only ROS/Eigen for this */
@@ -119,6 +131,17 @@ tf::Transform whycon::AxisSetter::compute_similarity(const std::vector<tf::Point
 	ROS_INFO_STREAM("transformed circle along Y -> " << similarity * points[2]);
 	ROS_INFO_STREAM("transformed circle along XY -> " << similarity * points[3]);
 
+	/*float original_x = (points[1] - points[0]).length();
+	float original_y = (points[2] - points[0]).length();
+	float x = (similarity * points[1] - similarity * points[0]).length();
+	float y = (similarity * points[2] - similarity * points[0]).length();
+	float scaling_x = xscale / x;
+	float scaling_y = yscale / y;
+
+	float xy = (scaling_x * (similarity * points[3]) - scaling_y * (similarity * points[0])).length();
+	ROS_INFO_STREAM("original X " << original_x << " original Y " << original_y << " norm X " << x << " norm Y " << y);
+	ROS_INFO_STREAM("obtained XY scale: " << xy << " expected: " << sqrt(xscale * xscale + yscale * yscale) << " scaling: " << scaling_x << " " << scaling_y);*/
+
 	return similarity;
 }
 
@@ -158,14 +181,19 @@ void whycon::AxisSetter::on_poses(const geometry_msgs::PoseArrayConstPtr& poses_
 {
 	if (transforms_set) return;
 
-	if (poses_msg->poses.size() != 4)
-	{
-		ROS_WARN_STREAM("Received " << poses_msg->poses.size() << " points. Four expected.");
+	if (poses_msg->poses.size() < 4) {
+		ROS_WARN_STREAM("Not computing, only " << poses_msg->poses.size() << " targets detected, need four.");
 		return;
 	}
 
-	std::vector<tf::Point> points;
-	detect_square(poses_msg->poses, points);
+	std::vector<tf::Point> points(4);
+	for (int i = 0; i < 4; i++)
+		tf::pointMsgToTF(poses_msg->poses[i].position, points[i]);
+
+	if (axis_order.empty())
+		detect_square(points);
+	else
+		build_square(points);
 
 	tf::Matrix3x3 projection = compute_projection(points, xscale, yscale);
 	tf::Transform similarity = compute_similarity(points);

src/ros/set_axis.h

@@ -21,13 +21,16 @@ namespace whycon {
 			double xscale, yscale;
 			bool transforms_set;
 
+			std::vector<int> axis_order;
+
 		private:
-			void detect_square(const std::vector<geometry_msgs::Pose>& msg_poses, std::vector<tf::Point>& points);
+			void detect_square(std::vector<tf::Point>& points);
 			tf::Matrix3x3 compute_projection(const std::vector<tf::Point>& points, float xscale, float yscale);
 			tf::Transform compute_similarity(const std::vector<tf::Point>& points);
 
 			void write_projection(YAML::Emitter& yaml, const tf::Matrix3x3& projection);
 			void write_similarity(YAML::Emitter& yaml, const tf::Transform& similarity);
+			void build_square(std::vector<tf::Point>& points);
 	};
 }
 

src/ros/whycon_ros.cpp

@@ -108,6 +108,9 @@ void whycon::WhyConROS::publish_results(const std_msgs::Header& header, const cv
       ostr << std::fixed << std::setprecision(2);
 			ostr << coord << " " << i;
       circle.draw(output_image, ostr.str(), cv::Vec3b(0,255,255));
+			/*whycon::CircleDetector::Circle new_circle = circle.improveEllipse(cv_ptr->image);
+			new_circle.draw(output_image, ostr.str(), cv::Vec3b(0,255,0));*/
+			cv::circle(output_image, camera_model.project3dToPixel(cv::Point3d(coord)), 1, cv::Vec3b(255,0,255), 1, CV_AA);
     }
 
     if (publish_poses) {