diff --git a/include/visual_gps_alg.h b/include/visual_gps_alg.h index 69cdc244b614a00a3258ba6e85c2e3f89a209507..f17d86aaacc89dd939783ba39c1abebb29155694 100644 --- a/include/visual_gps_alg.h +++ b/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); diff --git a/include/visual_gps_alg_node.h b/include/visual_gps_alg_node.h index f2592ee6481ea808e26613a16204780c1e6f5f27..9ae8336cd502f60c1c49e27b3984ad7834d5dcc9 100644 --- a/include/visual_gps_alg_node.h +++ b/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 * diff --git a/src/visual_gps_alg.cpp b/src/visual_gps_alg.cpp index 739b1839152a980a1c55ab6dc14d49a0cb48a264..c607d24185fea13f17ceebf8d8304f06fe6a884c 100644 --- a/src/visual_gps_alg.cpp +++ b/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++) diff --git a/src/visual_gps_alg_node.cpp b/src/visual_gps_alg_node.cpp index 22d02e9394e4ea78dd47493e6177a6a8e607f224..1c4cfdda91265edb077c5d8e94b718836479474c 100644 --- a/src/visual_gps_alg_node.cpp +++ b/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;