Eigen::Isometry3d getRelativeTransform(TagMatch const& match, Eigen::Matrix3d const & camera_matrix, const Eigen::Vector4d &distortion_coefficients, double tag_size)
{
std::vector<cv::Point3f> objPts;
std::vector<cv::Point2f> imgPts;
double s = tag_size/2.;
objPts.push_back(cv::Point3f(-s,-s, 0));
objPts.push_back(cv::Point3f( s,-s, 0));
objPts.push_back(cv::Point3f( s, s, 0));
objPts.push_back(cv::Point3f(-s, s, 0));
imgPts.push_back(match.p0);
imgPts.push_back(match.p1);
imgPts.push_back(match.p2);
imgPts.push_back(match.p3);
cv::Mat rvec, tvec;
cv::Matx33f cameraMatrix(
camera_matrix(0,0), 0, camera_matrix(0,2),
0, camera_matrix(1,1), camera_matrix(1,2),
0, 0, 1);
cv::Vec4f distParam(distortion_coefficients(0), distortion_coefficients(1), distortion_coefficients(2), distortion_coefficients(3));
cv::solvePnP(objPts, imgPts, cameraMatrix, distParam, rvec, tvec);
cv::Matx33d r;
cv::Rodrigues(rvec, r);
Eigen::Matrix3d wRo;
wRo << r(0,0), r(0,1), r(0,2), r(1,0), r(1,1), r(1,2), r(2,0), r(2,1), r(2,2);
Eigen::Isometry3d T;
T.linear() = wRo;
T.translation() << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
return T;
}
/**
* Computes the pose using homography matrix (NOT WORKING PROPERLY)
* @param contours contours from ellipse
*/
bool Circular::estimatePoseHom(nav_msgs::Odometry &odom_estimated)
{
std::vector<cv::Point3f> circle;
cv::Mat rvec, tvec;
// Assemble circle in the object plane
double radius = this->landmark_diag_ / 2.0;
for (int i=0; i<4; ++i){
double theta = atan2(this->contours_.at(i).y - this->descriptor_.dynamic.vc,
this->contours_.at(i).x - this->descriptor_.dynamic.uc);
circle.push_back(cv::Point3f(radius*cos(theta),-radius*sin(theta),0.0));
}
// Camera Calibration Matrix
cv::Matx33f cameraMatrix(
this->fx_, 0 , this->cx_,
0 ,this->fy_ , this->cy_,
0 , 0 , 1 );
// Distortion parameters
cv::Vec4f distParam(this->k1_, this->k2_, this->p1_, this->p2_); // distortion parameters
// PnP solver
cv::solvePnP(circle, this->contours_, cameraMatrix, distParam, rvec, tvec);
// Rodrigues to rotation matrix
cv::Matx33d r;
cv::Rodrigues(rvec, r);
Eigen::Matrix3d wRo;
wRo << r(0,0), r(0,1), r(0,2), r(1,0), r(1,1), r(1,2), r(2,0), r(2,1), r(2,2);
Eigen::Matrix4d T, transform;
transform.topLeftCorner(3,3) = wRo;
transform.col(3).head(3) << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(2);
transform.row(3) << 0,0,0,1;
Eigen::Matrix3d rot = wRo;
Eigen::Quaternion<double> rot_quaternion = Eigen::Quaternion<double>(rot);
odom_estimated.pose.pose.position.x = transform(0,3);
odom_estimated.pose.pose.position.y = transform(1,3);
odom_estimated.pose.pose.position.z = transform(2,3);
odom_estimated.pose.pose.orientation.x = rot_quaternion.x();
odom_estimated.pose.pose.orientation.y = rot_quaternion.y();
odom_estimated.pose.pose.orientation.z = rot_quaternion.z();
odom_estimated.pose.pose.orientation.w = rot_quaternion.w();
}
