NEWMAT::Matrix BFMatrix::SubMatrix(unsigned int fr, unsigned int lr, unsigned int fc, unsigned int lc) const { if (fr<1 || fc<1 || lr>Nrows() || lc>Ncols() || fr>lr || fc>lc) throw BFMatrixException("BFMatrix::SubMatrix: index out of range"); NEWMAT::Matrix omat(lr-fr+1,lc-fc+1); for (unsigned int r=fr, ri=1; r<=lr; r++, ri++) { for (unsigned int c=fc, ci=1; c<=lc; c++, ci++) { omat(ri,ci) = this->Peek(r,c); } } return(omat); }
void homographyToPoseCV(at::real fx, at::real fy, at::real tagSize, const at::Mat& horig, cv::Mat& rvec, cv::Mat& tvec) { at::Point3 opoints[4] = { at::Point3(-1, -1, 0), at::Point3( 1, -1, 0), at::Point3( 1, 1, 0), at::Point3(-1, 1, 0) }; at::Point ipoints[4]; at::real s = 0.5*tagSize; for (int i=0; i<4; ++i) { ipoints[i] = project(horig, opoints[i].x, opoints[i].y); opoints[i] *= s; } at::real Kdata[9] = { fx, 0, 0, 0, fy, 0, 0, 0, 1 }; at::Mat Kmat(3, 3, Kdata); at::Mat dcoeffs = at::Mat::zeros(4, 1); cv::Mat_<at::Point3> omat(4, 1, opoints); cv::Mat_<at::Point> imat(4, 1, ipoints); cv::Mat r, t; cv::solvePnP(omat, imat, Kmat, dcoeffs, r, t); if (rvec.type() == CV_32F) { r.convertTo(rvec, rvec.type()); } else { rvec = r; } if (tvec.type() == CV_32F) { t.convertTo(tvec, tvec.type()); } else { tvec = t; } }