void doTranslation(){
cout<<"doTranslation begin "<<endl;
//Mat m = (Mat_<float>(3,3) << 1.4572068353989741e+003, 0, 3.1950000000000000e+002,0, 1.4572068353989741e+003, 2.3950000000000000e+002,0, 0, 1);
cv::Mat tempMat, tempMat2;
double s, zConst = 0;
rotationMatrix.convertTo(rotationMatrix,CV_64FC1);
cameraMatrix.convertTo(cameraMatrix,CV_64FC1);
tempMat = rotationMatrix.inv() * cameraMatrix.inv() * uvPoint;
tempMat2 = rotationMatrix.inv() * tvec;
s = zConst + tempMat2.at<double>(2,0);
s /= tempMat.at<double>(2,0);
cout<<"s"<<s<<endl ;
cv::Mat wcPoint = rotationMatrix.inv() * (s * cameraMatrix.inv() * uvPoint - tvec);
Point3f realPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), wcPoint.at<double>(2, 0));
cout<<"image point "<<endl<<uvPoint<< endl<< "to real point "<<endl <<wcPoint<<endl<<endl ;
/*static vector<Point2d> imagePoints_hand ;
static vector<double> timeStampList_hand ;
static vector<Point3f> realWorldPoints_hand ;
static vector<int> frameNumList_hand ;*/
imagePoints_hand.push_back(Point2d(uvPoint.at<double>(0,0),uvPoint.at<double>(1,0)));
realWorldPoints_hand.push_back(realPoint);
frameNumList_hand.push_back(frameNum_hand);
}
double searchAngle(const Mat &src, const Mat &dst, double st, double ed, double step, char dir, double &mpsnr) {
int captureWidth = src.cols, captureHeight = src.rows;
//const double fuvX = 799, fuvY = 799;
const double fuvX = 744.5, fuvY = 744.5;
double cx = captureWidth / 2 - 0.5, cy = captureHeight / 2 - 0.5;
Mat K = Mat::zeros(3, 3, CV_64F);
K.at<double>(0, 0) = fuvX;
K.at<double>(1, 1) = fuvY;
K.at<double>(0, 2) = cx;
K.at<double>(1, 2) = cy;
K.at<double>(2, 2) = 1;
double max = -100, angle = 0;
for (double x = st; x >= ed; x -= step) {
Mat R = VideoStabilization::rotationMat(angle2Quaternion(x * (dir == 0), x * (dir == 1), 0));
Mat H = K * R.inv() * K.inv(), src2, dst2, diff;
warpPerspective(dst, dst2, H, Size(src.cols, src.rows), INTER_LINEAR + WARP_INVERSE_MAP);
src.copyTo(src2, dst2);
double psnr = getPSNR(src2, dst2);
if (psnr > max) {
max = psnr;
angle = x;
}
}
mpsnr = max;
return angle;
}
void rectify(Mat& K1, Mat& R1, Mat& t1,
Mat& K2, Mat& R2, Mat& t2,
Size size,
Mat& T1, Mat& T2) {
Mat_<double> Kn1(3, 3), Kn2(3, 3);
Mat_<double> c1(3, 1), c2(3, 1),
v1(3, 1), v2(3, 1), v3(3, 1);
K1.copyTo(Kn1);
K2.copyTo(Kn2);
// optical centers
c1 = - R1.t() * t1;
c2 = - R2.t() * t2;
// x axis
v1 = c2 - c1;
// y axis
v2 = Mat(R1.row(2).t()).cross(v1);
// z axis
v3 = v1.cross(v2);
Mat_<double> m_R(3, 3);
v1 = v1 / norm(v1);
v2 = v2 / norm(v2);
v3 = v3 / norm(v3);
m_R(0, 0) = v1(0), m_R(0, 1) = v1(1), m_R(0, 2) = v1(2);
m_R(1, 0) = v2(0), m_R(1, 1) = v2(1), m_R(1, 2) = v2(2);
m_R(2, 0) = v3(0), m_R(2, 1) = v3(1), m_R(2, 2) = v3(2);
T1 = (Kn1 * m_R) * (R1.t() * K1.inv());
T2 = (Kn2 * m_R) * (R2.t() * K2.inv());
// Image center displacement
Mat_<double> o1(3, 1), o2(3, 1);
o1(0) = 0.5 * size.width, o1(1) = 0.5 * size.height, o1(2) = 1.0;
o2(0) = 0.5 * size.width, o2(1) = 0.5 * size.height, o2(2) = 1.0;
Mat_<double> on1(3, 1), on2(3, 1), d1(2, 1), d2(2, 1);
on1 = T1 * o1;
on2 = T2 * o2;
d1(0) = o1(0) - on1(0) / on1(2);
d1(1) = o1(1) - on1(1) / on1(2);
d2(0) = o2(0) - on2(0) / on1(2);
d2(1) = o2(1) - on2(1) / on1(2);
d1(1) = d2(1);
Kn1(0, 2) = Kn1(0, 2) + d1(0);
Kn1(1, 2) = Kn1(1, 2) + d1(1);
Kn2(0, 2) = Kn2(0, 2) + d2(0);
Kn2(1, 2) = Kn2(1, 2) + d2(1);
T1 = (Kn1 * m_R) * (R1.t() * K1.inv());
T2 = (Kn2 * m_R) * (R2.t() * K2.inv());
}
void doTranslation() {
cout<<"doTranslation begin "<<endl;
//Mat m = (Mat_<float>(3,3) << 1.4572068353989741e+003, 0, 3.1950000000000000e+002,0, 1.4572068353989741e+003, 2.3950000000000000e+002,0, 0, 1);
cv::Mat tempMat, tempMat2;
double s, zConst = 0;
rotationMatrix.convertTo(rotationMatrix,CV_64FC1);
cameraMatrix.convertTo(cameraMatrix,CV_64FC1);
tempMat = rotationMatrix.inv() * cameraMatrix.inv() * uvPoint;
tempMat2 = rotationMatrix.inv() * tvec;
s = zConst + tempMat2.at<double>(2,0);
s /= tempMat.at<double>(2,0);
/* double m11,m12,m14,m21,m22,m24,m31,m32,m34 ;
double u,v;
double ay,ax,by,bx,c ;
m11 = rotationMatrix.at<double>(0,0);
m12 = rotationMatrix.at<double>(0,1);
m21 = rotationMatrix.at<double>(1,0);
m22 = rotationMatrix.at<double>(1,1);
m31 = rotationMatrix.at<double>(2,0);
m32 = rotationMatrix.at<double>(2,1);
m14 = tvec.at<double>(0,0);
m24 = tvec.at<double>(1,0);
m34 = tvec.at<double>(2,0);
u = uvPoint.at<double>(0,0);
v = uvPoint.at<double>(1,0);
c= m12*m21-m11*m22 ;
ay = m21*u-m11*v ;
by = m14*m21 - m11*m24 ;
ax = m22*u-m12*v ;
bx = m14*m22-m12*m24 ;
s = ((m31*bx-m32*by)/c+m34)/(1+(m31*ax-m32*ay)/c);*/
cout<<"s"<<s<<endl ;
cv::Mat wcPoint = rotationMatrix.inv() * (s * cameraMatrix.inv() * uvPoint - tvec);
Point3f realPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), wcPoint.at<double>(2, 0));
cout<<"image point "<<endl<<uvPoint<< endl<< "to real point "<<endl <<wcPoint<<endl<<endl ;
}
void adaptXYZToNewWhitePoint(Mat& XYZWhitePoint, const Mat& BGRWhitePoint){
Mat sRGBChromCoord = (Mat_<float>(3, 3) << 0.6400, 0.3000, 0.1500,
0.3300, 0.6000, 0.0600,
0.2126, 0.7152, 0.0722);
Mat invsRGBChromCoord = sRGBChromCoord.inv();
Mat XYZD65 = (Mat_<float>(1, 3) << 0.9642, 1.0000, 0.8249);
Mat xyWhitePoint = Mat(BGRWhitePoint.rows, BGRWhitePoint.cols, CV_32FC3);
for (int k = 0; k < 3; k++){
xyWhitePoint.at<Vec3f>(0, 0).val[k] = product(XYZD65, invsRGBChromCoord, k);
}
Mat T = (Mat_<float>(3, 3) << xyWhitePoint.at<Vec3f>(0, 0).val[0], 0, 0,
0, xyWhitePoint.at<Vec3f>(0, 0).val[1], 0,
0, 0, xyWhitePoint.at<Vec3f>(0, 0).val[2]);
Mat tmpMat = Mat(3, 3, CV_32F);
dotProduct(sRGBChromCoord, T, tmpMat);
Mat linBGRWhitePoint = Mat(BGRWhitePoint.rows, BGRWhitePoint.cols, CV_32FC3);
for (int i = 0; i < 3; i++){
linBGRWhitePoint.at<Vec3f>(0, 0).val[i] =
sRGBInverseCompanding(BGRWhitePoint.at<Vec3f>(0, 0).val[i]);
}
for (int k = 0; k < 3; k++){
XYZWhitePoint.at<Vec3f>(0, 0).val[k] =
linBGRWhitePoint.at<Vec3f>(0, 0).val[2] * tmpMat.at<float>(k, 0) +
linBGRWhitePoint.at<Vec3f>(0, 0).val[1] * tmpMat.at<float>(k, 1) +
linBGRWhitePoint.at<Vec3f>(0, 0).val[0] * tmpMat.at<float>(k, 2);
}
}
bool computeBGRGrayWhitePoint(const Mat& bgrImg, Mat& whitePoint,
Mat& prevYUVWhitePoint){
float sumElementsByChannel[2] = {0, 0};
int cntElm = 0;
Mat matrix = (Mat_<float>(3, 3) << 0.299, 0.587, 0.114,
-0.229, -0.587, 0.886,
0.701, -0.587, -0.114);
Mat invMatrix = matrix.inv();
Mat curYUVWhitePoint = Mat(1, 1, CV_32FC3);
Mat yuvImg = Mat(bgrImg.rows, bgrImg.cols, CV_32FC3);
convertBGRtoYUV(bgrImg, yuvImg);
for (int i = 0; i < yuvImg.rows; i++){
for (int j = 0; j < yuvImg.cols; j++){
float y = yuvImg.at<Vec3f>(i, j).val[0];
float u = yuvImg.at<Vec3f>(i, j).val[1];
float v = yuvImg.at<Vec3f>(i, j).val[2];
float ratio = (abs(u) + abs(v)) / y;
if (ratio < GREY_THRESHOLD){
sumElementsByChannel[0] += u;
sumElementsByChannel[1] += v;
cntElm++;
}
}
}
curYUVWhitePoint.at<Vec3f>(0, 0).val[0] = 1;
curYUVWhitePoint.at<Vec3f>(0, 0).val[1] = sumElementsByChannel[0] / cntElm;
curYUVWhitePoint.at<Vec3f>(0, 0).val[2] = sumElementsByChannel[1] / cntElm;
// termination criteria
float curY = curYUVWhitePoint.at<Vec3f>(0, 0).val[0];
float curU = curYUVWhitePoint.at<Vec3f>(0, 0).val[1];
float curV = curYUVWhitePoint.at<Vec3f>(0, 0).val[2];
float prevU = prevYUVWhitePoint.at<Vec3f>(0, 0).val[1];
float prevV = prevYUVWhitePoint.at<Vec3f>(0, 0).val[2];
float norm = sqrt(pow(curU - prevU, 2) + pow(curV - prevV, 2));
if (!std::isnan(prevU) && (norm < pow(10, -6) ||
std::max(abs(curU), abs(curV)) < 0.001)){
return true;
}
prevYUVWhitePoint.at<Vec3f>(0, 0).val[0] = curY;
prevYUVWhitePoint.at<Vec3f>(0, 0).val[1] = curU;
prevYUVWhitePoint.at<Vec3f>(0, 0).val[2] = curV;
for (int i = 0; i < 3; i++){
whitePoint.at<Vec3f>(0, 0).val[2 - i] = invMatrix.at<float>(i, 0) * curY +
invMatrix.at<float>(i, 1) * curU +
invMatrix.at<float>(i, 2) * curV;
whitePoint.at<Vec3f>(0, 0).val[2 - i] =
clipImg(whitePoint.at<Vec3f>(0, 0).val[2 - i], 0, 1);
}
return false;
}
/// If H is not orientation preserving at the point, the error is infinite.
/// For this test, it is assumed that det(H)>0.
/// \param H The homography matrix.
/// \param index The correspondence index
/// \param side In which image is the error measured?
/// \return The square reprojection error.
double HomographyModel::Error(const Mat &H, size_t index, int* side) const {
double err = std::numeric_limits<double>::infinity();
if(side) *side=1;
libNumerics::vector<double> x(3);
// Transfer error in image 2
x(0) = x1_(0,index);
x(1) = x1_(1,index);
x(2) = 1.0;
x = H * x;
if(x(2)>0) {
x /= x(2);
err = sqr(x2_(0,index)-x(0)) + sqr(x2_(1,index)-x(1));
}
// Transfer error in image 1
if(symError_) { // ... but only if requested
x(0) = x2_(0,index);
x(1) = x2_(1,index);
x(2) = 1.0;
x = H.inv() * x;
if(x(2)>0) {
x /= x(2);
double err1 = sqr(x1_(0,index)-x(0)) + sqr(x1_(1,index)-x(1));
if(err1>err) { // Keep worse error
err=err1;
if(side) *side=0;
}
}
}
return err;
}
// ----------------------------------------------------------------
// Применяем преобразование к точкам рамки
// ----------------------------------------------------------------
void transformPts(vector<Point2f>& src_pts,Mat& Tau,vector<Point2f>& dst_pts)
{
vector<Point2f> pts(4);
double w=fabs(src_pts[0].x-src_pts[2].x);
double h=fabs(src_pts[0].y-src_pts[2].y);
pts.assign(src_pts.begin(),src_pts.end());
// Нашли центр
Point2f center=(pts[0]+pts[2])*0.5;
Mat tmp(4,1,CV_64FC2);
for(int i=0;i<4;i++)
{
tmp.at<Vec2d>(i)[0]=pts[i].x-center.x;
tmp.at<Vec2d>(i)[1]=pts[i].y-center.y;
}
Mat Tau_inv=Tau.inv();
perspectiveTransform(tmp,tmp,Tau_inv);
for(int i=0;i<4;i++)
{
pts[i].x=tmp.at<Vec2d>(i)[0]+center.x;
pts[i].y=tmp.at<Vec2d>(i)[1]+center.y;
}
dst_pts.assign(pts.begin(),pts.end());
}
PoseRT PoseRT::inv() const
{
Mat projectiveMatrix = getProjectiveMatrix();
Mat invertedProjectiveMatrix = projectiveMatrix.inv(DECOMP_SVD);
PoseRT invertedPose(invertedProjectiveMatrix);
return invertedPose;
}
cv::Mat AntiShake::fixPictures(Mat &img_1, Mat &img_2, int loops, double final_pic_size,
double maxDetDiff, int featurePoints, int coreSize, double absoluteRelation, int matches_type) {
// Firstly we calculate the Homography matrix and refine it in the FeedbackController function:
Mat H = calcHomographyFeedbackController(img_1, img_2, loops, final_pic_size, featurePoints, coreSize,
absoluteRelation, matches_type);
double det = determinant(H);
cout << "STEP 11 final homography = " << endl << H << endl << " determinant = " << det << endl;
//Secondly, lets transform the picture according to the calculated (resultant) smatrix.
//TODO
/*
* Create white Image with the same size as img_1 and img_2
* apply Homography on white one
* Count histogramas and pixels with value == 0 (black pixels)
* if value > 0, crop on both img_1 and img_2
*
* */
if (det >= 1.0) {
applyHomography(H, img_1, img_2);
cout << "STEP 12 fixed original pictures 1->2" << endl;
return H;
} else {
Mat H2 = H.inv();
H.release();
applyHomography(H2, img_2, img_1);
cout << "STEP 12 fixed original pictures 2->1 " << endl;
return H2;
}
}
// -----------------------------------------------------------------------
/// \brief Initialise et calcule la matrice fondamentale.
///
/// @param mLeftIntrinsic: matrice intrinseque de la camera gauche
/// @param mLeftExtrinsic: matrice extrinseque de la camera gauche
/// @param mRightIntrinsic: matrice intrinseque de la camera droite
/// @param mRightExtrinsic: matrice extrinseque de la camera droite
/// @return matrice fondamentale
// -----------------------------------------------------------------------
Mat iviFundamentalMatrix(const Mat& mLeftIntrinsic,
const Mat& mLeftExtrinsic,
const Mat& mRightIntrinsic,
const Mat& mRightExtrinsic) {
// A modifier !
// Doit utiliser la fonction iviVectorProductMatrix
Mat mFundamental = Mat::eye(3, 3, CV_64F);
Mat tmp = (Mat_<double>(3,4) <<
1.0, 0.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0,
0.0, 0.0, 1.0, 0.0
);
Mat P1 = mLeftIntrinsic * tmp * mLeftExtrinsic;
Mat P2 = mRightIntrinsic * tmp * mRightExtrinsic;
Mat O = mLeftExtrinsic.inv();
Mat O1 = (Mat_<double>(4,1) <<
O.at<double>(3),
O.at<double>(7),
O.at<double>(11),
O.at<double>(15)
);
Mat Hpi = P2 * P1.inv(DECOMP_SVD);
mFundamental = iviVectorProductMatrix(P2*O1) * Hpi;
// Retour de la matrice fondamentale
return mFundamental;
}
void ProjectorBase::setCameraParams(InputArray _K, InputArray _R, InputArray _T)
{
Mat K = _K.getMat(), R = _R.getMat(), T = _T.getMat();
CV_Assert(K.size() == Size(3, 3) && K.type() == CV_32F);
CV_Assert(R.size() == Size(3, 3) && R.type() == CV_32F);
CV_Assert((T.size() == Size(1, 3) || T.size() == Size(3, 1)) && T.type() == CV_32F);
Mat_<float> K_(K);
k[0] = K_(0,0); k[1] = K_(0,1); k[2] = K_(0,2);
k[3] = K_(1,0); k[4] = K_(1,1); k[5] = K_(1,2);
k[6] = K_(2,0); k[7] = K_(2,1); k[8] = K_(2,2);
Mat_<float> Rinv = R.t();
rinv[0] = Rinv(0,0); rinv[1] = Rinv(0,1); rinv[2] = Rinv(0,2);
rinv[3] = Rinv(1,0); rinv[4] = Rinv(1,1); rinv[5] = Rinv(1,2);
rinv[6] = Rinv(2,0); rinv[7] = Rinv(2,1); rinv[8] = Rinv(2,2);
Mat_<float> R_Kinv = R * K.inv();
r_kinv[0] = R_Kinv(0,0); r_kinv[1] = R_Kinv(0,1); r_kinv[2] = R_Kinv(0,2);
r_kinv[3] = R_Kinv(1,0); r_kinv[4] = R_Kinv(1,1); r_kinv[5] = R_Kinv(1,2);
r_kinv[6] = R_Kinv(2,0); r_kinv[7] = R_Kinv(2,1); r_kinv[8] = R_Kinv(2,2);
Mat_<float> K_Rinv = K * Rinv;
k_rinv[0] = K_Rinv(0,0); k_rinv[1] = K_Rinv(0,1); k_rinv[2] = K_Rinv(0,2);
k_rinv[3] = K_Rinv(1,0); k_rinv[4] = K_Rinv(1,1); k_rinv[5] = K_Rinv(1,2);
k_rinv[6] = K_Rinv(2,0); k_rinv[7] = K_Rinv(2,1); k_rinv[8] = K_Rinv(2,2);
Mat_<float> T_(T.reshape(0, 3));
t[0] = T_(0,0); t[1] = T_(1,0); t[2] = T_(2,0);
}
void ProjectorBase::setCameraParams(const Mat &K, const Mat &R, const Mat &T)
{
CV_Assert(K.size() == Size(3, 3) && K.type() == CV_32F);
CV_Assert(R.size() == Size(3, 3) && R.type() == CV_32F);
CV_Assert((T.size() == Size(1, 3) || T.size() == Size(3, 1)) && T.type() == CV_32F);
Mat_<float> K_(K);
k[0] = K_(0,0); k[1] = K_(0,1); k[2] = K_(0,2);
k[3] = K_(1,0); k[4] = K_(1,1); k[5] = K_(1,2);
k[6] = K_(2,0); k[7] = K_(2,1); k[8] = K_(2,2);
Mat_<float> Rinv = R.t();
rinv[0] = Rinv(0,0); rinv[1] = Rinv(0,1); rinv[2] = Rinv(0,2);
rinv[3] = Rinv(1,0); rinv[4] = Rinv(1,1); rinv[5] = Rinv(1,2);
rinv[6] = Rinv(2,0); rinv[7] = Rinv(2,1); rinv[8] = Rinv(2,2);
Mat_<float> R_Kinv = R * K.inv();
r_kinv[0] = R_Kinv(0,0); r_kinv[1] = R_Kinv(0,1); r_kinv[2] = R_Kinv(0,2);
r_kinv[3] = R_Kinv(1,0); r_kinv[4] = R_Kinv(1,1); r_kinv[5] = R_Kinv(1,2);
r_kinv[6] = R_Kinv(2,0); r_kinv[7] = R_Kinv(2,1); r_kinv[8] = R_Kinv(2,2);
Mat_<float> K_Rinv = K * Rinv;
k_rinv[0] = K_Rinv(0,0); k_rinv[1] = K_Rinv(0,1); k_rinv[2] = K_Rinv(0,2);
k_rinv[3] = K_Rinv(1,0); k_rinv[4] = K_Rinv(1,1); k_rinv[5] = K_Rinv(1,2);
k_rinv[6] = K_Rinv(2,0); k_rinv[7] = K_Rinv(2,1); k_rinv[8] = K_Rinv(2,2);
Mat_<float> T_(T.reshape(0, 3));
t[0] = T_(0,0); t[1] = T_(1,0); t[2] = T_(2,0);
}
void denormalize(Mat &H, const Mat &Hn, const Mat &T, const Mat &Tp)
{
H.create(Hn.rows, Hn.cols, Hn.type());
H = T.inv()*Hn*Tp;
H /= H.at<float>(H.rows-1, H.cols-1);
}
Mat draw_image_in_quad(Mat& image, vector<Point>& quad, const Mat& imposed, Mat homography = Mat())
{
vector<Point> src_points;
src_points.push_back(Point(0, 0));
src_points.push_back(Point(imposed.cols, 0));
src_points.push_back(Point(imposed.cols, imposed.rows));
src_points.push_back(Point(0, imposed.rows));
if (homography.rows == 0 || homography.cols == 0)
{
homography = findHomography(src_points, quad);
}
Mat homography_inv = homography.inv();
Rect r = boundingRect(quad);
for (int i = 0; i < r.height; ++i)
{
for (int j = 0; j < r.width; ++j)
{
int x = j + r.x;
int y = i + r.y;
// if (x, y) is in the quad
if (pointPolygonTest(quad, Point(x, y), false) > -0.5)
{
// lookup pixel value
// H * src == dest ->
// src ~= H^-1 * dest
Mat p = Mat::zeros(3, 1, CV_64F);
p.at<double>(0, 0) = x;
p.at<double>(1, 0) = y;
p.at<double>(2, 0) = 1;
Mat q = homography_inv * p;
double wx = q.at<double>(0, 0);
double wy = q.at<double>(1, 0);
double w = q.at<double>(2, 0);
assert(w != 0.0f);
int qx = (int)(round(wx / w));
int qy = (int)(round(wy / w));
if (qx >= 0 && qy >= 0 && qx < imposed.cols && qy < imposed.rows)
{
// printf("(%d, %d) -> (%d, %d)\n", y, x, qy, qx);
image.at<Vec3b>(y, x) = imposed.at<Vec3b>(qy, qx);
}
else
{
// cout << qx << ", " << qy << endl;
}
}
}
}
return homography;
}
void InversePerspectiveMapping::updateFrameUsingStandardAssumption(const Mat &pose){
rCW2CI = pose.inv() * firstFrameParam.rtMat;
//cout << " pose " << pose << endl;
rCW2UV = firstFrameParam.intrMat * rCW2CI;
//cout << " rCW2UV " << rCW2UV << endl;
calcRemapMat();
}
static Mat getFundamentalMat( const Mat& R1, const Mat& t1,
const Mat& R2, const Mat& t2,
const Mat& cameraMatrix )
{
Mat_<double> R = R2*R1.t(), t = t2 - R*t1;
double tx = t.at<double>(0,0), ty = t.at<double>(1,0), tz = t.at<double>(2,0);
Mat E = (Mat_<double>(3,3) << 0, -tz, ty, tz, 0, -tx, -ty, tx, 0)*R;
Mat iK = cameraMatrix.inv();
Mat F = iK.t()*E*iK;
#if 0
static bool checked = false;
if(!checked)
{
vector<Point3f> objpoints(100);
Mat O(objpoints);
randu(O, Scalar::all(-10), Scalar::all(10));
vector<Point2f> imgpoints1, imgpoints2;
projectPoints(Mat(objpoints), R1, t1, cameraMatrix, Mat(), imgpoints1);
projectPoints(Mat(objpoints), R2, t2, cameraMatrix, Mat(), imgpoints2);
double* f = (double*)F.data;
for( size_t i = 0; i < objpoints.size(); i++ )
{
Point2f p1 = imgpoints1[i], p2 = imgpoints2[i];
double diff = p2.x*(f[0]*p1.x + f[1]*p1.y + f[2]) +
p2.y*(f[3]*p1.x + f[4]*p1.y + f[5]) +
f[6]*p1.x + f[7]*p1.y + f[8];
CV_Assert(fabs(diff) < 1e-3);
}
checked = true;
}
#endif
return F;
};
Point3f AugmentedView::getCameraRotation()
{
/*Mat invTranslation = Mat::eye(4,4,CV_32F);
OpenGLHelper::translate(invTranslation,Point3f(((float)position->at<double>(0,0)), ((float)position->at<double>(0,1)),-((float)position->at<double>(0,2))));
*/
Mat cameraProj = Mat::eye(4,4,CV_32F);
OpenGLHelper::gluPerspective(cameraProj,fieldOfView,1.7f,20.0f, 600.0f);
Mat invCamProj = cameraProj.inv();
//Projection = camera * gyro * translation * rotation
Mat rotationOnly = projection * invCamProj;
Mat rotationVector = Mat(1,3,CV_32F);
rotationOnly *= (1.0f / rotationOnly.at<float>(3,3));
Mat smallRotation = rotationOnly(Range(0,3),Range(0,3));
try
{
LOGD_Mat(LOGTAG_POSITION,"smallRotation",&smallRotation);
cv::Rodrigues(smallRotation,rotationVector);
LOGD_Mat(LOGTAG_POSITION,"RotationVector",&rotationVector);
}
catch (exception & e)
{
LOGW(LOGTAG_POSITION,"Error rodriguing: %s",e.what());
return Point3f(0,0,0);
}
return Point3f(-(rotationVector.at<float>(0,0)), -(rotationVector.at<float>(0,1)),-(rotationVector.at<float>(0,2)));
}
//------------------------------ Update ---------------------------------
void KalmanFilter::Update(const cv::Mat &z) {
CHECK(z.rows == H_.rows);
Mat y = z - H_ * predicted_x_; // Innovation.
Mat S = H_ * predicted_P_ * H_.t() + R_; // Innovation covariance.
Mat K = predicted_P_ * H_.t() * S.inv(); // Optimal Kalman gain.
x_ = predicted_x_ + K * y; // A posteriori state estimate.
Mat I = cv::Mat::eye(P_.rows, P_.cols, CV_32F);
P_ = (I - K * H_) * predicted_P_; // Updated state covariance.
}
vector<Point2f> Recognition::projectPointsBack(vector<Point2f> mesh, Mat& originalImage, Mat homography){
vector<Point2f> reprojectedPoints;
Mat homoInverse=homography.inv();
perspectiveTransform(mesh, reprojectedPoints, homoInverse);
/* for(unsigned int i=0; i< mesh.size(); i++){
circle(originalImage, reprojectedPoints[i], 1, Scalar(0,0,255), 2);
}*/
return reprojectedPoints;
}
//----------------------------------------------------------
//
//----------------------------------------------------------
void PutBanner(Mat& img,Mat& banner,Mat& Tau,vector<Point2f>& region_tau)
{
Point2f center=(region_tau[0]+region_tau[2])*0.5;
Rect roi=cv::boundingRect(region_tau);
double w=roi.width;
double h=roi.height;
vector<Point2f> pts(w*h);
vector<Point2f> dst_pts(w*h);
Mat tmp(w*h,1,CV_64FC2);
int ind=0;
for(int i=roi.y;i<roi.y+roi.height;i++)
{
for(int j=roi.x;j<roi.x+roi.width;j++)
{
tmp.at<Vec2d>(ind)[0]=j-roi.x-roi.width/2.0;
tmp.at<Vec2d>(ind)[1]=i-roi.y-roi.height/2.0;
ind++;
}
}
Mat Tau_inv=Tau.inv();
perspectiveTransform(tmp,tmp,Tau);
ind=0;
for(int i=roi.y;i<roi.y+roi.height;i++)
{
for(int j=roi.x;j<roi.x+roi.width;j++)
{
Vec2d val=tmp.at<Vec2d>(ind);
pts[ind].x=val[0]+banner.cols/2.0;
pts[ind].y=val[1]+banner.rows/2.0;
ind++;
}
}
dst_pts.assign(pts.begin(),pts.end());
ind=0;
for(int i=roi.y;i<roi.y+roi.height;i++)
{
for(int j=roi.x;j<roi.x+roi.width;j++)
{
if(dst_pts[ind].y>0 && dst_pts[ind].y<banner.rows && dst_pts[ind].x >0 && dst_pts[ind].x <banner.cols)
{
img.at<Vec3b>(i,j)=banner.at<Vec3b>(dst_pts[ind].y,dst_pts[ind].x);
}
ind++;
}
}
}
void calibExt(double *para, vector<Mat> Hset, Mat& K)
{
int imgNum = Hset.size();
Mat h1(3, 1, CV_64FC1, Scalar(0)), h2(3, 1, CV_64FC1, Scalar(0)), h3(3, 1, CV_64FC1, Scalar(0));
Mat invK, r1, r2, r3, t;
Mat Q(3, 3, CV_64FC1, Scalar(0));
Mat U, V, D, Vt, Ut, R, Rnew(3, 1, CV_64FC1, Scalar(1));
double norm;
invK = K.inv();
cout << "invK: " << invK << endl;
for(int i = 0; i < imgNum; i++)
{
for(int j = 0; j < 3; j++){
h1.at<double>(j, 0) = Hset[i].at<double>(j, 0);
h2.at<double>(j, 0) = Hset[i].at<double>(j, 1);
h3.at<double>(j, 0) = Hset[i].at<double>(j, 2);
}
//cout << "h1: " << h1 << endl;
r1 = invK*h1;
norm = sqrt(r1.dot(r1));
rvecNorm(r1, norm);
//cout << "r1: " << r1 << endl;
r2 = invK*h2;
rvecNorm(r2, norm);
//cout << "r2: " << r2 << endl;
r3 = r1.cross(r2);
rvecNorm(r3, norm);
//cout << "r3: " << r3 << endl;
t = invK*h3;
rvecNorm(t, norm);
//cout << "t: " << t << endl;
// rotation conditioning
for(int k = 0; k < 3; k++)
{
Q.at<double>(k, 0) = r1.at<double>(k, 0);
Q.at<double>(k, 1) = r2.at<double>(k, 0);
Q.at<double>(k, 2) = r3.at<double>(k, 0);
}
//cout << "Q: " << Q << endl;
SVD::compute(Q, D, U, Vt, 0);
//cout << "D: " << D << endl;
// rotaion conditioning
R = U*Vt;
//cout << "R: " << R.t()*R << endl;
// convert to rodrigues representation
Rodrigues(R, Rnew);
//cout << "Rnew: " << Rnew << endl;
para[7 + i*6] = Rnew.at<double>(0);
para[7 + i*6 + 1] = Rnew.at<double>(1);
para[7 + i*6 + 2] = Rnew.at<double>(2);
para[7 + i*6 + 3] = t.at<double>(0);
para[7 + i*6 + 4] = t.at<double>(1);
para[7 + i*6 + 5] = t.at<double>(2);
}
}
//TODO this implementation is suboptimal
void Image::invert(Image& dst){
#if IMAGE_SAFE == 1
assert(dst.hasSize(cols,rows));
#endif
Mat A = this->toMat();
Mat B = A.inv(DECOMP_SVD);
memcpy((void*) dst.data.get(),(void*) B.data, sizeof(float)*cols*rows);
}
static Mat LieSub(Mat A, Mat B){
Mat Pa;
Mat Pb;
LieToP(A,Pa);
LieToP(B,Pb);
Mat out;
CV_Assert(A.size()==Size(6,1) && B.size()==Size(6,1));
PToLie(Pa*Pb.inv(),out);
return out;
}
Mat Assignment2::warpImage(Mat H)
{
// Apply perspective transformation to see effect of H
Mat hinv = H.inv(DECOMP_LU);
Mat translated_image;
warpPerspective(this->image, translated_image, hinv, this->image.size(), CV_INTER_LINEAR, BORDER_CONSTANT);
return translated_image;
}
void dataProcess(int start, int end, int step)
{
bool first = true;
Mat A[640], B[640];
double depth[640];
int index = 1;
for (int s = start; s <= end; s += step)
{
static ifstream infile;
if (!infile.is_open()) {
infile.open("depth2.txt", ios::in);
}
for (int i = 0; i < 640; i++)
{
double a;
infile >> a;
/*if (a<0 || a>5000)
a = 0;*/
depth[i] = a;
//cout << a << " ";
}
for (int i = 0; i < 640; i++)
{
double num = depth[i];
//double temp[3] = { num*num, num, 1 };
double temp[2] = { num, 1 };
A[i ].push_back(Mat(1, 2, CV_64FC1, temp));
/*double temp[2] = { num, 1 };
A[i].push_back(Mat(1, 2, CV_64FC1, temp));*/
B[i ].push_back(Mat(1, 1, CV_64FC1, s));
}
}
/*Mat P = A[140].t()*A[140];
Mat Q = A[140].t()*B[140];
cout << P << endl;
cout << Q << endl;
cout << P.inv()*Q << endl;*/
for (int i = 0; i < 640; i++)
{
Mat AA = A[i].t()* A[i];
Mat BB = A[i].t()* B[i];
Mat x = AA.inv()*BB;
cout << x << endl;
a[i] = x.at<double>(0, 0);
b[i] = x.at<double>(1, 0);
//c[i] = x.at<double>(2, 0);
}
}
static Point3f getObject3DCoord(Point3f objectPosition, Mat projection)
{
Mat invProjection = projection.inv();
float data[] = {objectPosition.x,objectPosition.y,objectPosition.z,1.0f};
Mat cameraPos = Mat(4,1,CV_32F,data);
cameraPos = invProjection * cameraPos;
cameraPos *= 1.0f/ cameraPos.at<float>(3,0);
LOG_Mat(ANDROID_LOG_VERBOSE,LOGTAG_ARINPUT,"3D->Screen",&cameraPos);
return Point3f(cameraPos.at<float>(0,0),cameraPos.at<float>(1,0),cameraPos.at<float>(2,0));
}
Point2f backProject(Point2f p, Mat cameraMatrix, Mat rotationMatrix, Mat tvec, float height)
{
cv::Mat uvPoint = cv::Mat::ones(3, 1, cv::DataType<double>::type); // [u v 1]
uvPoint.at<double>(0, 0) = p.x;
uvPoint.at<double>(1, 0) = p.y;
cv::Mat tempMat, tempMat2;
double s;
tempMat = rotationMatrix.inv() * cameraMatrix.inv() * uvPoint;
tempMat2 = rotationMatrix.inv() * tvec;
s = height + tempMat2.at<double>(2, 0); //height Zconst is zero
s /= tempMat.at<double>(2, 0);
Mat pt = rotationMatrix.inv() * (s * cameraMatrix.inv() * uvPoint - tvec);
//printf("[%f %f %f]\n", pt.at<double>(0, 0), pt.at<double>(1, 0), pt.at<double>(2, 0));
//cv::Mat backPt = 1 / s * cameraMatrix * (rotationMatrix * pt + tvec);
//printf("[%f %f]\n", backPt.at<double>(0, 0), backPt.at<double>(1, 0));
return Point2f((float)pt.at<double>(0, 0), (float)pt.at<double>(1, 0));
}
static Point3f getCameraPosition(Mat projection)
{
Mat invProjection = projection.inv();
float data[] = {0,0,0,1.0f};
Mat cameraPos = Mat(4,1,CV_32F,data);
cameraPos = invProjection * cameraPos;
cameraPos *= 1.0f/ cameraPos.at<float>(3,0);
return Point3f(cameraPos.at<float>(0,0),cameraPos.at<float>(1,0),cameraPos.at<float>(2,0));
//LOG_Mat(ANDROID_LOG_VERBOSE,LOGTAG_ARINPUT,"CameraPosition",&cameraPos);
}
Point3f doTranslate(Point2d imagePoint){
uvPoint.at<double>(0,0) = imagePoint.x+0.0; //got this point using mouse callback
uvPoint.at<double>(1,0) = imagePoint.y+0.0;
cv::Mat tempMat, tempMat2;
double s, zConst = 0;
cout<<"DoTranslate ("<<uvPoint.at<double>(0,0)<<","<<uvPoint.at<double>(1,0)<<") ->";
tempMat = rotationMatrix.inv() * cameraMatrix.inv() * uvPoint;
tempMat2 = rotationMatrix.inv() * tvec;
s = zConst + tempMat2.at<double>(2,0);
s /= tempMat.at<double>(2,0);
cv::Mat wcPoint = rotationMatrix.inv() * (s * cameraMatrix.inv() * uvPoint - tvec);
Point3f realPoint(wcPoint.at<double>(0, 0), wcPoint.at<double>(1, 0), wcPoint.at<double>(2, 0));
cout<<"("<<realPoint.x<<","<<realPoint.y<<","<<realPoint.z<<") with s="<<s <<endl ;
return realPoint ;
}
