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;
}
}
