Loaded all the camera parameters to rectify only the pannel centroids.

Added a TF broadcaster to send the visual gps transform.

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 K,D,R,P;//,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);
@@ -125,7 +125,7 @@ class VisualGpsAlgorithm
 
     // here define all visual_gps_alg interface methods to retrieve and set
     // the driver parameters
-    void set_camera_parameters(double fx, double cx, double fy, double cy);
+    void set_camera_params(cv::Mat &K,cv::Mat &D,cv::Mat &R,cv::Mat &P,unsigned int width,unsigned int height);
     void compute_pos(std::vector<iri_blob_detector::blob> &blobs1,std::vector<iri_blob_detector::blob> &blobs2,std::vector<iri_blob_detector::blob> &blobs3,std::vector<iri_blob_detector::blob> &blobs4);
     void get_pose(geometry_msgs::Pose &pose);
 

include/visual_gps_alg_node.h

@@ -27,6 +27,7 @@
 
 #include <iri_base_algorithm/iri_base_algorithm.h>
 #include "visual_gps_alg.h"
+#include <tf/transform_broadcaster.h>
 
 // [publisher subscriber headers]
 #include <nav_msgs/Odometry.h>
@@ -101,6 +102,8 @@ class VisualGpsAlgNode : public algorithm_base::IriBaseAlgorithm<VisualGpsAlgori
 
     // [action client attributes]
 
+    tf::TransformBroadcaster odom_broadcaster;
+
    /**
     * \brief config variable
     *

src/visual_gps_alg.cpp

@@ -6,8 +6,6 @@ VisualGpsAlgorithm::VisualGpsAlgorithm(void)
 {
   pthread_mutex_init(&this->access_,NULL);
 
-  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;
@@ -56,17 +54,16 @@ void VisualGpsAlgorithm::update_points(std::vector<iri_blob_detector::blob> &blo
         max_size=blobs[i].size;
         max_index=i;
       }
-    points_img.push_back(cv::Point2f(blobs[max_index].center_x-cam_matrix.at<float>(0,2),blobs[max_index].center_y-cam_matrix.at<float>(1,2)));
+    points_img.push_back(cv::Point2f(blobs[max_index].center_x-this->K.at<double>(0,2),blobs[max_index].center_y-this->K.at<double>(1,2)));
   }
 }
 
-void VisualGpsAlgorithm::set_camera_parameters(double fx, double cx, double fy, double cy)
+void VisualGpsAlgorithm::set_camera_params(cv::Mat &K,cv::Mat &D,cv::Mat &R,cv::Mat &P,unsigned int width,unsigned int height)
 {
-  cam_matrix.at<float>(0,0) = fx;
-  cam_matrix.at<float>(0,2) = cx;
-  cam_matrix.at<float>(1,1) = fy;
-  cam_matrix.at<float>(1,2) = cy;
-  cam_matrix.at<float>(2,2) = 1.0f;
+  this->K=K;
+  this->D=D;
+  this->R=R;
+  this->P=P;
 }
 
 /*void VisualGpsAlgorithm::compute_pos(std::vector<iri_blob_detector::blob> &blobs1,std::vector<iri_blob_detector::blob> &blobs2,std::vector<iri_blob_detector::blob> &blobs3,std::vector<iri_blob_detector::blob> &blobs4)
@@ -115,6 +112,7 @@ void VisualGpsAlgorithm::compute_pos(std::vector<iri_blob_detector::blob> &blobs
 {
   std::vector<cv::Point3f> points_3d;
   std::vector<cv::Point2f> points_img;
+  std::vector<cv::Point2f> points_img_undist;
   Eigen::Vector2d point_a,point_b,ab_vector,center_world,tmp_vector;
   std::vector<Eigen::Vector2d> circle_center;
   std::vector<double> circle_radius;
@@ -124,6 +122,7 @@ void VisualGpsAlgorithm::compute_pos(std::vector<iri_blob_detector::blob> &blobs
   this->update_points(blobs2,points_3d,points_img);
   this->update_points(blobs4,points_3d,points_img);
   this->update_points(blobs3,points_3d,points_img);
+  cv::fisheye::undistortPoints(points_img,points_img_undist,this->K,this->D,this->R,this->P); 
   if(points_3d.size()>3 && points_img.size()>3)
   {
     for(unsigned int i=0;i<points_3d.size();i++)

src/visual_gps_alg_node.cpp

@@ -52,6 +52,8 @@ VisualGpsAlgNode::~VisualGpsAlgNode(void)
 
 void VisualGpsAlgNode::mainNodeThread(void)
 {
+  tf::Quaternion tf_quat;
+  geometry_msgs::TransformStamped odom_trans;
   // [fill msg structures]
   
   // [fill srv structure and make request to the server]
@@ -65,10 +67,19 @@ void VisualGpsAlgNode::mainNodeThread(void)
     this->alg_.lock();
     this->alg_.compute_pos(this->color1_blobs,this->color2_blobs,this->color3_blobs,this->color4_blobs);
     this->odom_Odometry_msg_.header.stamp=ros::Time::now();
-    this->odom_Odometry_msg_.header.frame_id="world";
-    this->odom_Odometry_msg_.child_frame_id="visual_gps";
+    this->odom_Odometry_msg_.header.frame_id=this->config_.world_frame;
+    this->odom_Odometry_msg_.child_frame_id=this->config_.odom_gps_frame;
     this->alg_.get_pose(this->odom_Odometry_msg_.pose.pose);
     this->odom_publisher_.publish(this->odom_Odometry_msg_);
+    // broadcast TF
+    odom_trans.header.frame_id = this->config_.world_frame;
+    odom_trans.child_frame_id = this->config_.odom_gps_frame;
+    odom_trans.header.stamp = ros::Time::now();
+    odom_trans.transform.translation.x = this->odom_Odometry_msg_.pose.pose.position.x;
+    odom_trans.transform.translation.y = this->odom_Odometry_msg_.pose.pose.position.y;
+    odom_trans.transform.translation.z = this->odom_Odometry_msg_.pose.pose.position.z;
+    odom_trans.transform.rotation=this->odom_Odometry_msg_.pose.pose.orientation;
+    this->odom_broadcaster.sendTransform(odom_trans);
     this->alg_.unlock();
   }
 }
@@ -78,12 +89,11 @@ void VisualGpsAlgNode::camera_info_callback(const sensor_msgs::CameraInfo::Const
 {
   this->camera_info_mutex_enter();
 
-  double fx,fy,cx,cy;
-  fx = msg->K[0];
-  fy = msg->K[4];
-  cx = msg->K[2];
-  cy = msg->K[5];
-  this->alg_.set_camera_parameters(fx,cx,fy,cy);
+  cv::Mat K=(cv::Mat_<double>(3,3) << msg->K[0] , msg->K[1] , msg->K[2] , msg->K[3] , msg->K[4] , msg->K[5] , msg->K[6] , msg->K[7] , msg->K[8]);
+  cv::Mat D=(cv::Mat_<double>(4,1) << msg->D[0] , msg->D[1] , msg->D[4] , 0.0);
+  cv::Mat R=(cv::Mat_<double>(3,3) << msg->R[0] , msg->R[1] , msg->R[2] , msg->R[3] , msg->R[4] , msg->R[5] , msg->R[6] , msg->R[7] , msg->R[8]);
+  cv::Mat P=(cv::Mat_<double>(3,4) << msg->P[0] , msg->P[1] , msg->P[2] , msg->P[3] , msg->P[4] , msg->P[5] , msg->P[6] , msg->P[7] , msg->P[8] , msg->P[9] , msg->P[10] , msg->P[11]);
+  this->alg_.set_camera_params(K,D,R,P,msg->width,msg->height);
   this->camera_info_subscriber_.shutdown();
   cam_info_received = true;