diff --git a/CMakeLists.txt b/CMakeLists.txt index b41d4ee0da527fd02ae7564d7e1935d8dc237110..0712c2f74b6cc6d5db0d2bf8be0ce1ddb1bef213 100644 --- a/CMakeLists.txt +++ b/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 # ******************************************************************** diff --git a/include/visual_gps_alg.h b/include/visual_gps_alg.h index 6906063d2fadee69b8903606602a194230f67703..69cdc244b614a00a3258ba6e85c2e3f89a209507 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 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); diff --git a/package.xml b/package.xml index a385547b70483bea474fb581d67249ce2b7f9d52..747361880bc71b7bd4f808f49e5dcf1c62233b05 100644 --- a/package.xml +++ b/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> diff --git a/src/visual_gps_alg.cpp b/src/visual_gps_alg.cpp index 5c3d297e4bbb7e685ecf3952c02fbff7838df213..739b1839152a980a1c55ab6dc14d49a0cb48a264 100644 --- a/src/visual_gps_alg.cpp +++ b/src/visual_gps_alg.cpp @@ -1,5 +1,6 @@ #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)