Skip to content
Snippets Groups Projects
Commit 6e9a8b2d authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

SolvePnP function not working. 

Implemented an other method to estimate the position of the robot.
parent d5b5b58e
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
include/visual_gps_alg.h
+ 1
− 1
View file @ 6e9a8b2d
......@@ -50,7 +50,7 @@ class VisualGpsAlgorithm
pthread_mutex_t access_;
// private attributes and methods
cv::Mat cam_matrix;
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);
......
src/visual_gps_alg.cpp
+ 86
− 10
View file @ 6e9a8b2d
#include "visual_gps_alg.h"
#include <Eigen/Dense>
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;
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)
......@@ -30,13 +40,13 @@ void VisualGpsAlgorithm::update_points(std::vector<iri_blob_detector::blob> &blo
if(blobs.size()!=0)// some blobs detected for color1
{
if(blobs[0].id.find(this->config_.color1_id))// blob 1 matches with color1
if(blobs[0].id.find(this->config_.color1_id)!=std::string::npos)// blob 1 matches with color1
points_3d.push_back(cv::Point3f(this->config_.color1_x,this->config_.color1_y,this->config_.color1_z));
else if(blobs[0].id.find(this->config_.color2_id))// blob 1 matches with color2
else if(blobs[0].id.find(this->config_.color2_id)!=std::string::npos)// blob 1 matches with color2
points_3d.push_back(cv::Point3f(this->config_.color2_x,this->config_.color2_y,this->config_.color2_z));
else if(blobs[0].id.find(this->config_.color3_id))// blob 1 matches with color2
else if(blobs[0].id.find(this->config_.color3_id)!=std::string::npos)// blob 1 matches with color3
points_3d.push_back(cv::Point3f(this->config_.color3_x,this->config_.color3_y,this->config_.color3_z));
else if(blobs[0].id.find(this->config_.color4_id))// blob 1 matches with color2
else if(blobs[0].id.find(this->config_.color4_id)!=std::string::npos)// blob 1 matches with color4
points_3d.push_back(cv::Point3f(this->config_.color4_x,this->config_.color4_y,this->config_.color4_z));
// get the biggest blob
for(unsigned int i=0;i<blobs.size();i++)
......@@ -45,7 +55,7 @@ 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,blobs[max_index].center_y));
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)));
}
}
......@@ -58,20 +68,24 @@ void VisualGpsAlgorithm::set_camera_parameters(double fx, double cx, double fy,
cam_matrix.at<float>(2,2) = 1.0f;
}
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)
/*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)
{
std::vector<cv::Point3f> points_3d;
std::vector<cv::Point2f> points_img;
cv::Mat tvec,rvec,R;
double w,x,y,z;
cv::Mat R;
this->update_points(blobs1,points_3d,points_img);
this->update_points(blobs2,points_3d,points_img);
this->update_points(blobs3,points_3d,points_img);
this->update_points(blobs4,points_3d,points_img);
if(points_3d.size()>2 && points_img.size()>2)
this->update_points(blobs3,points_3d,points_img);
if(points_3d.size()>3 && points_img.size()>3)
{
if(cv::solvePnP(cv::Mat(points_3d),cv::Mat(points_img),this->cam_matrix,cv::Mat(),rvec,tvec,false,CV_ITERATIVE))
for(unsigned int i=0;i<points_3d.size();i++)
std::cout << points_3d[i].x << "," << points_3d[i].y << "," << points_3d[i].z << "," << "->" << points_img[i].x << "," << points_img[i].y << std::endl;
//if(cv::solvePnP(cv::Mat(points_3d),cv::Mat(points_img),this->cam_matrix,cv::Mat(),this->rvec,this->tvec,true,CV_ITERATIVE))
//if(cv::solvePnP(cv::Mat(points_3d),cv::Mat(points_img),this->cam_matrix,cv::Mat(),this->rvec,this->tvec,true,CV_P3P))
if(cv::solvePnP(cv::Mat(points_3d),cv::Mat(points_img),this->cam_matrix,cv::Mat(),this->rvec,this->tvec,true,CV_EPNP))
{
cv::Rodrigues(rvec,R);
// Matrix to quaternion
......@@ -90,7 +104,69 @@ void VisualGpsAlgorithm::compute_pos(std::vector<iri_blob_detector::blob> &blobs
this->pose.orientation.y = y;
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;
}
}
}*/
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)
{
std::vector<cv::Point3f> points_3d;
std::vector<cv::Point2f> points_img;
Eigen::Vector2d point_a,point_b,ab_vector,center_world,tmp_vector;
std::vector<Eigen::Vector2d> circle_center;
std::vector<double> circle_radius;
double angle,angle_a,angle_b,radius;
this->update_points(blobs1,points_3d,points_img);
this->update_points(blobs2,points_3d,points_img);
this->update_points(blobs4,points_3d,points_img);
this->update_points(blobs3,points_3d,points_img);
if(points_3d.size()>3 && points_img.size()>3)
{
for(unsigned int i=0;i<points_3d.size();i++)
{
for(unsigned int j=i+1;j<points_3d.size();j++)
{
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)
angle_a+=2*3.14159;
angle_b=atan2(-points_img[j].y,points_img[j].x);
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;
radius=ab_vector.norm()/(2.0*fabs(cos(angle)));
circle_radius.push_back(radius);
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;
}
}
}
Eigen::MatrixXd A(circle_center.size()-1,2);
Eigen::VectorXd B(circle_center.size()-1);
Eigen::Vector2d sol;
for(unsigned int i=0;i<circle_center.size()-1;i++)
{
A(i,0)=circle_center[i+1](0)-circle_center[0](0);
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;
}
}
......
src/visual_gps_alg_node.cpp
+ 1
− 1
View file @ 6e9a8b2d
......@@ -83,7 +83,7 @@ void VisualGpsAlgNode::camera_info_callback(const sensor_msgs::CameraInfo::Const
fy = msg->K[4];
cx = msg->K[2];
cy = msg->K[5];
this->alg_.set_camera_parameters(fx,fy,cx,cy);
this->alg_.set_camera_parameters(fx,cx,fy,cy);
this->camera_info_subscriber_.shutdown();
cam_info_received = true;
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment