void PatternTrackingInfo::computePose(const Pattern& pattern, const CameraCalibration& calibration)
{
cv::Mat Rvec;
cv::Mat_<float> Tvec;
cv::Mat raux,taux;
cv::solvePnP(pattern.points3d, points2d, calibration.getIntrinsic(), calibration.getDistorsion(),raux,taux);
raux.convertTo(Rvec,CV_32F);
taux.convertTo(Tvec ,CV_32F);
cv::Mat_<float> rotMat(3,3);
cv::Rodrigues(Rvec, rotMat);
// Copy to transformation matrix
for (int col=0; col<3; col++)
{
for (int row=0; row<3; row++)
{
pose3d.r().mat[row][col] = rotMat(row,col); // Copy rotation component
}
pose3d.t().data[col] = Tvec(col); // Copy translation component
}
// Since solvePnP finds camera location, w.r.t to marker pose, to get marker pose w.r.t to the camera we invert it.
pose3d = pose3d.getInverted();
}
MarkerDetector::MarkerDetector(CameraCalibration calibration)
: m_minContourLengthAllowed(100)
, markerSize(100,100)
{
cv::Mat(3,3, CV_32F, const_cast<float*>(&calibration.getIntrinsic().data[0])).copyTo(camMatrix);
cv::Mat(4,1, CV_32F, const_cast<float*>(&calibration.getDistorsion().data[0])).copyTo(distCoeff);
bool centerOrigin = true;
if (centerOrigin)
{
m_markerCorners3d.push_back(cv::Point3f(-0.5f,-0.5f,0));
m_markerCorners3d.push_back(cv::Point3f(+0.5f,-0.5f,0));
m_markerCorners3d.push_back(cv::Point3f(+0.5f,+0.5f,0));
m_markerCorners3d.push_back(cv::Point3f(-0.5f,+0.5f,0));
}
else
{
m_markerCorners3d.push_back(cv::Point3f(0,0,0));
m_markerCorners3d.push_back(cv::Point3f(1,0,0));
m_markerCorners3d.push_back(cv::Point3f(1,1,0));
m_markerCorners3d.push_back(cv::Point3f(0,1,0));
}
m_markerCorners2d.push_back(cv::Point2f(0,0));
m_markerCorners2d.push_back(cv::Point2f(markerSize.width-1,0));
m_markerCorners2d.push_back(cv::Point2f(markerSize.width-1,markerSize.height-1));
m_markerCorners2d.push_back(cv::Point2f(0,markerSize.height-1));
}