cv::Mat GoodFrame::getRT_Clark(PoseEsti* poseEsti)
{
glm::mat4 rotateMatrix = poseEsti->poseEstimation(this->getDetected_landmarks());
cv::Mat _pm(4, 4, CV_64FC1);
_pm.at<double>(0,0) = rotateMatrix[0][0];
_pm.at<double>(0,1) = rotateMatrix[0][1];
_pm.at<double>(0,2) = rotateMatrix[0][2];
_pm.at<double>(0,3) = rotateMatrix[0][3];
_pm.at<double>(1,0) = rotateMatrix[1][0];
_pm.at<double>(1,1) = rotateMatrix[1][1];
_pm.at<double>(1,2) = rotateMatrix[1][2];
_pm.at<double>(1,3) = rotateMatrix[1][3];
_pm.at<double>(2,0) = rotateMatrix[2][0];
_pm.at<double>(2,1) = rotateMatrix[2][1];
_pm.at<double>(2,2) = rotateMatrix[2][2];
_pm.at<double>(2,3) = rotateMatrix[2][3];
_pm.at<double>(3,0) = rotateMatrix[3][0];
_pm.at<double>(3,1) = rotateMatrix[3][1];
_pm.at<double>(3,2) = rotateMatrix[3][2];
_pm.at<double>(3,3) = rotateMatrix[3][3];
return _pm;
}
cv::Mat GoodFrame::getRT(PoseEsti* poseEsti)
{
// cout<<"getRT======TODO : poseEsti->poseEstimation"<<endl;
poseEsti->poseEstimation(this->getDetected_landmarks());
glm::mat4 rotateMatrix = poseEsti->rotateMatrix;
// cout<<"=================rotateMatrix"<<endl<<
// rotateMatrix[0][0]<<" "<<rotateMatrix[0][1]<<" "<<rotateMatrix[0][2]<<" "<<rotateMatrix[0][3]<<endl<<
// rotateMatrix[1][0]<<" "<<rotateMatrix[1][1]<<" "<<rotateMatrix[1][2]<<" "<<rotateMatrix[1][3]<<endl<<
// rotateMatrix[2][0]<<" "<<rotateMatrix[2][1]<<" "<<rotateMatrix[2][2]<<" "<<rotateMatrix[2][3]<<endl<<
// rotateMatrix[3][0]<<" "<<rotateMatrix[3][1]<<" "<<rotateMatrix[3][2]<<" "<<rotateMatrix[3][3]<<endl;
//cout<<"Mean_Feature_distance: "<<(float)poseEsti->Mean_Feature_distance<<endl;
//cout<<"init_Mean_Feature_distance: "<<(float)poseEsti->init_Mean_Feature_distance<<endl;
//cout<<"scale: "<<poseEsti->scale<<endl;
//cv::Mat _pm(4, 3, CV_64FC1);
//_pm.at<double>(0,0) = rotateMatrix[0][0]; _pm.at<double>(0,1) = rotateMatrix[0][1]; _pm.at<double>(0,2) = rotateMatrix[0][2];
//_pm.at<double>(1,0) = rotateMatrix[1][0]; _pm.at<double>(1,1) = rotateMatrix[1][1]; _pm.at<double>(1,2) = rotateMatrix[1][2];
//_pm.at<double>(2,0) = rotateMatrix[2][0]; _pm.at<double>(2,1) = rotateMatrix[2][1]; _pm.at<double>(2,2) = rotateMatrix[2][2];
//_pm.at<double>(3,0) = rotateMatrix[3][0]; _pm.at<double>(3,1) = rotateMatrix[3][1]; _pm.at<double>(3,2) = rotateMatrix[3][2];
cv::Mat _pm(4, 4, CV_64FC1);
_pm.at<double>(0,0) = rotateMatrix[0][0];
_pm.at<double>(0,1) = rotateMatrix[0][1];
_pm.at<double>(0,2) = rotateMatrix[0][2];
_pm.at<double>(0,3) = rotateMatrix[0][3];
_pm.at<double>(1,0) = rotateMatrix[1][0];
_pm.at<double>(1,1) = rotateMatrix[1][1];
_pm.at<double>(1,2) = rotateMatrix[1][2];
_pm.at<double>(1,3) = rotateMatrix[1][3];
_pm.at<double>(2,0) = rotateMatrix[2][0];
_pm.at<double>(2,1) = rotateMatrix[2][1];
_pm.at<double>(2,2) = rotateMatrix[2][2];
_pm.at<double>(2,3) = rotateMatrix[2][3];
_pm.at<double>(3,0) = rotateMatrix[3][0];
_pm.at<double>(3,1) = rotateMatrix[3][1];
_pm.at<double>(3,2) = rotateMatrix[3][2];
_pm.at<double>(3,3) = rotateMatrix[3][3];
// cout<<"getRT====== end"<<endl;
return _pm;
}
cv::Mat GoodFrame::getRT(std::vector<cv::Point3f> &modelPoints_min)
{
cv::Mat img = this->getCapturesImage();
std::vector<cv::Point2f> imagePoints;
for (size_t i = 0; i < 68; ++i)
{
imagePoints.push_back(cv::Point2f((float)(this->getDetected_landmarks().at<double>(i)),
(float)(this->getDetected_landmarks().at<double>(i+68))));
}
/////
int max_d = MAX(img.rows,img.cols);
cv::Mat camMatrix = (Mat_<double>(3,3) << max_d, 0, img.cols/2.0,
0, max_d, img.rows/2.0,
0, 0, 1.0);
cv::Mat ip(imagePoints);
cv::Mat op(modelPoints_min);
std::vector<double> rv(3), tv(3);
cv::Mat rvec(rv),tvec(tv);
double _dc[] = {0,0,0,0};
// std::cout << ip << std::endl << std::endl;
// std::cout << op << std::endl << std::endl;
// std::cout << camMatrix << std::endl << std::endl;
solvePnP(op, ip, camMatrix, cv::Mat(1,4,CV_64FC1,_dc), rvec, tvec, false, CV_EPNP);
double rot[9] = {0};
cv::Mat rotM(3, 3, CV_64FC1, rot);
cv::Rodrigues(rvec, rotM);
double* _r = rotM.ptr<double>();
// printf("rotation mat: \n %.3f %.3f %.3f\n%.3f %.3f %.3f\n%.3f %.3f %.3f\n",_r[0],_r[1],_r[2],_r[3],_r[4],_r[5],_r[6],_r[7],_r[8]);
// printf("trans vec: \n %.3f %.3f %.3f\n",tv[0],tv[1],tv[2]);
cv::Mat _pm(3, 4, CV_64FC1);
_pm.at<double>(0,0) = _r[0]; _pm.at<double>(0,1) = _r[1]; _pm.at<double>(0,2) = _r[2]; _pm.at<double>(0,3) = tv[0];
_pm.at<double>(1,0) = _r[3]; _pm.at<double>(1,1) = _r[4]; _pm.at<double>(1,2) = _r[5]; _pm.at<double>(1,3) = tv[1];
_pm.at<double>(2,0) = _r[6]; _pm.at<double>(2,1) = _r[7]; _pm.at<double>(2,2) = _r[8]; _pm.at<double>(2,3) = tv[2];
return _pm;
}
void server_info (struct roar_connection * con) {
struct roar_server_info * info = roar_server_info(con);
if ( info == NULL ) {
fprintf(stderr, "Error: can not get server info\n");
return;
}
_pm(version, "version");
_pm(location, "location");
_pm(description, "description");
_pm(contact, "contact");
_pm(serial, "serial");
_pm(address, "address");
_pm(uiurl, "UI URL");
_pm(un.sysname, "System sysname");
_pm(un.release, "System release");
_pm(un.nodename, "System nodename");
_pm(un.machine, "System machine");
roar_server_info_free(info);
}
