Added the estimation of the orientation.

CMakeLists.txt

@@ -6,7 +6,7 @@ find_package(catkin REQUIRED)
 # ******************************************************************** 
 #                 Add catkin additional components here
 # ******************************************************************** 
-find_package(catkin REQUIRED COMPONENTS iri_base_algorithm nav_msgs iri_blob_detector sensor_msgs geometry_msgs dynamic_reconfigure message_generation)
+find_package(catkin REQUIRED COMPONENTS iri_base_algorithm nav_msgs iri_blob_detector sensor_msgs geometry_msgs dynamic_reconfigure message_generation tf)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
@@ -62,7 +62,7 @@ catkin_package(
 # ******************************************************************** 
 #            Add ROS and IRI ROS run time dependencies
 # ******************************************************************** 
- CATKIN_DEPENDS iri_base_algorithm nav_msgs iri_blob_detector sensor_msgs geometry_msgs dynamic_reconfigure message_runtime
+ CATKIN_DEPENDS iri_base_algorithm nav_msgs iri_blob_detector sensor_msgs geometry_msgs dynamic_reconfigure message_runtime tf
 # ******************************************************************** 
 #      Add system and labrobotica run time dependencies here
 # ******************************************************************** 

include/visual_gps_alg.h

@@ -50,7 +50,7 @@ class VisualGpsAlgorithm
     pthread_mutex_t access_;    
 
     // private attributes and methods
-    cv::Mat cam_matrix,tvec,rvec;
+    cv::Mat cam_matrix;//,tvec,rvec;
     geometry_msgs::Pose pose; 
 
     void update_points(std::vector<iri_blob_detector::blob> &blobs, std::vector<cv::Point3f> &points_3d,std::vector<cv::Point2f> &points_img);

package.xml

@@ -56,6 +56,7 @@
   <build_depend>geometry_msgs</build_depend>
   <build_depend>dynamic_reconfigure</build_depend>
   <build_depend>message_generation</build_depend>
+  <build_depend>tf</build_depend>
   <run_depend>iri_base_algorithm</run_depend>
   <run_depend>nav_msgs</run_depend>
   <run_depend>iri_blob_detector</run_depend>
@@ -63,6 +64,7 @@
   <run_depend>geometry_msgs</run_depend>
   <run_depend>dynamic_reconfigure</run_depend>
   <run_depend>message_runtime</run_depend>
+  <run_depend>tf</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

src/visual_gps_alg.cpp

 #include "visual_gps_alg.h"
 #include <Eigen/Dense>
+#include <tf/tf.h>
 
 VisualGpsAlgorithm::VisualGpsAlgorithm(void)
 {
@@ -7,14 +8,14 @@ VisualGpsAlgorithm::VisualGpsAlgorithm(void)
 
   this->cam_matrix = cv::Mat(3,3,CV_32FC1,cv::Scalar::all(0));
 
-  this->rvec.create(1,3, CV_32F);
-  this->rvec.at<double>(0)=0.0;
-  this->rvec.at<double>(1)=0.0;
-  this->rvec.at<double>(2)=0.0;
-  this->tvec.create(1,3, CV_32F);
-  this->tvec.at<double>(0)=0.0;
-  this->tvec.at<double>(1)=0.0;
-  this->tvec.at<double>(2)=0.0;
+//  this->rvec.create(1,3, CV_32F);
+//  this->rvec.at<double>(0)=0.0;
+//  this->rvec.at<double>(1)=0.0;
+//  this->rvec.at<double>(2)=0.0;
+//  this->tvec.create(1,3, CV_32F);
+//  this->tvec.at<double>(0)=0.0;
+//  this->tvec.at<double>(1)=0.0;
+//  this->tvec.at<double>(2)=0.0;
 }
 
 VisualGpsAlgorithm::~VisualGpsAlgorithm(void)
@@ -105,7 +106,7 @@ void VisualGpsAlgorithm::set_camera_parameters(double fx, double cx, double fy,
       this->pose.orientation.z = z;
       this->pose.orientation.w = w;
       std::cout << this->pose.position.x << "," << this->pose.position.y << "," << this->pose.position.z << std::endl;
-//      std::cout << this->pose.orientation.x << "," << this->pose.orientation.y << "," << this->pose.orientation.z << "," << this->pose.orientation.w << std::endl;
+      std::cout << this->pose.orientation.x << "," << this->pose.orientation.y << "," << this->pose.orientation.z << "," << this->pose.orientation.w << std::endl;
     }
   }
 }*/
@@ -131,8 +132,6 @@ void VisualGpsAlgorithm::compute_pos(std::vector<iri_blob_detector::blob> &blobs
       {
         point_a << points_3d[i].x , points_3d[i].y;
         point_b << points_3d[j].x , points_3d[j].y;
-        std::cout << "Point A:" << points_3d[i].x << "," << points_3d[i].y << std::endl;
-        std::cout << "Point B:" << points_3d[j].x << "," << points_3d[j].y << std::endl;
         ab_vector=point_b-point_a;
         angle_a=atan2(-points_img[i].y,points_img[i].x);
         if(angle_a<0.0)
@@ -141,7 +140,6 @@ void VisualGpsAlgorithm::compute_pos(std::vector<iri_blob_detector::blob> &blobs
         if(angle_b<0.0)
           angle_b+=2*3.14159;
         angle=angle_a-angle_b;
-        std::cout << angle << std::endl;
         if(fabs(cos(angle))<0.9)
         {
           angle=3.14159/2.0-angle;
@@ -150,7 +148,6 @@ void VisualGpsAlgorithm::compute_pos(std::vector<iri_blob_detector::blob> &blobs
           tmp_vector << -ab_vector(1) , ab_vector(0);
           center_world=point_a+(ab_vector+tan(angle)*tmp_vector)/2.0;
           circle_center.push_back(center_world);
-          std::cout << radius << "," << center_world(0) << "," << center_world(1) << std::endl;
         }
       }
     }
@@ -163,11 +160,33 @@ void VisualGpsAlgorithm::compute_pos(std::vector<iri_blob_detector::blob> &blobs
       A(i,1)=circle_center[i+1](1)-circle_center[0](1);
       B(i)=pow(circle_center[i+1](0),2)-pow(circle_center[0](0),2)+(pow(circle_center[i+1](1),2)-pow(circle_center[0](1),2)+pow(circle_radius[0],2)-pow(circle_radius[i+1],2))/2.0;
     }
-    std::cout << A << std::endl;
-    std::cout << B << std::endl;
     sol=(A.transpose()*A).inverse()*A.transpose()*B;
-    std::cout << sol(0) << "," << sol(1) << std::endl;
+    this->pose.position.x=sol(0);
+    this->pose.position.y=sol(1);
+    this->pose.position.z=0.0;
   }
+  // estimate orientation
+  double camera_x_avg=0.0,camera_y_avg=0.0,world_x_avg=0.0,world_y_avg=0.0,yaw;
+  double num=0.0,den=0.0;
+
+  for(unsigned int i=0;i<points_3d.size();i++)
+  {
+    camera_x_avg+=points_img[i].x;
+    camera_y_avg+=points_img[i].y;
+    world_x_avg+=points_3d[i].x;
+    world_y_avg+=points_3d[i].y;
+  }
+  camera_x_avg/=points_3d.size();
+  camera_y_avg/=points_3d.size();
+  world_x_avg/=points_3d.size();
+  world_y_avg/=points_3d.size();
+  for(unsigned int i=0;i<points_3d.size();i++)
+  {
+    num+=(points_img[i].x-camera_x_avg)*(points_3d[i].y-world_y_avg)-(points_img[i].y-camera_y_avg)*(points_3d[i].x-world_x_avg);
+    den+=(points_img[i].x-camera_x_avg)*(points_3d[i].x-world_x_avg)-(points_img[i].y-camera_y_avg)*(points_3d[i].y-world_y_avg);
+  }
+  yaw=atan2(num,den);
+  this->pose.orientation=tf::createQuaternionMsgFromYaw(yaw);
 }
 
 void VisualGpsAlgorithm::get_pose(geometry_msgs::Pose &pose)