double QuadQuality(const Vertex & a,const Vertex &b,const Vertex &c,const Vertex &d)
{
// calcul de 4 angles --
R2 A((R2)a),B((R2)b),C((R2)c),D((R2)d);
R2 AB(B-A),BC(C-B),CD(D-C),DA(A-D);
// Move(A),Line(B),Line(C),Line(D),Line(A);
const Metric & Ma = a;
const Metric & Mb = b;
const Metric & Mc = c;
const Metric & Md = d;
double lAB=Norme2(AB);
double lBC=Norme2(BC);
double lCD=Norme2(CD);
double lDA=Norme2(DA);
AB /= lAB; BC /= lBC; CD /= lCD; DA /= lDA;
// version aniso
double cosDAB= Ma(DA,AB)/(Ma(DA)*Ma(AB)),sinDAB= Det(DA,AB);
double cosABC= Mb(AB,BC)/(Mb(AB)*Mb(BC)),sinABC= Det(AB,BC);
double cosBCD= Mc(BC,CD)/(Mc(BC)*Mc(CD)),sinBCD= Det(BC,CD);
double cosCDA= Md(CD,DA)/(Md(CD)*Md(DA)),sinCDA= Det(CD,DA);
double sinmin=Min(Min(sinDAB,sinABC),Min(sinBCD,sinCDA));
// cout << A << B << C << D ;
// cout << " sinmin " << sinmin << " " << cosDAB << " " << cosABC << " " << cosBCD << " " << cosCDA << endl;
// rattente(1);
if (sinmin<=0) return sinmin;
return 1.0-Max(Max(Abs(cosDAB),Abs(cosABC)),Max(Abs(cosBCD),Abs(cosCDA)));
}
void Calib::computeProjectionMatrix()
{
std::cout << "1111111111***********************************************************************************\n";
// for(int i = 0;i < imagePoints.size();i++)
// {
// std::cout<<" imagePoints[i]= "<< imagePoints[i]<<std::endl;
// std::cout<<" objectPoints[i]= "<< objectPoints[i]<<std::endl;
// }
if (objectPoints.size() != imagePoints.size())
return;
int n = objectPoints.size();
Eigen::Vector2d avg2;
Eigen::Vector3d avg3;
for (unsigned int i = 0; i < n; i++)
{
avg2 += imagePoints[i];
avg3 += objectPoints[i];
std::cout << i << " " << objectPoints[i](0) << " " << objectPoints[i](1) << " " << objectPoints[i](2) << std::endl;
}
avg2 = avg2 / n;
avg3 = avg3 / n;
std::cout << "avg2 = " << avg2 << std::endl;
std::cout << "avg3 = " << avg3 << std::endl;
/* *******************************************************************************
* Data normalization
* Translates and normalises a set of 2D homogeneous points so that their centroid is at the origin and their mean distance from the origin is sqrt(2).
*/
float meandist2 = 0;
float meandist3 = 0;
imagePoints_t.resize(n);
objectPoints_t.resize(n);
for (unsigned int i = 0; i < n; i++)
{
imagePoints_t[i] = imagePoints[i] - avg2;
objectPoints_t[i] = objectPoints[i] - avg3;
// std::cout << "1 imagePoints_t[i] = " << imagePoints_t[i] << std::endl;
// std::cout << "1 objectPoints_t[i] = " << objectPoints_t[i] << std::endl;
meandist2 += sqrt(imagePoints_t[i](0) * imagePoints_t[i](0) + imagePoints_t[i](1) * imagePoints_t[i](1));
meandist3 += sqrt(objectPoints_t[i](0) * objectPoints_t[i](0) + objectPoints_t[i](1) * objectPoints_t[i](1)
+ objectPoints_t[i](2) * objectPoints_t[i](2));
}
meandist2 /= n;
meandist3 /= n;
std::cout << "meandist2 = " << meandist2 << std::endl;
std::cout << "meandist3 = " << meandist3 << std::endl;
float scale2 = sqrt(2) / meandist2;
float scale3 = sqrt(3) / meandist3;
std::cout << "2222222222222***********************************************************************************\n";
for (unsigned int i = 0; i < n; i++)
{
imagePoints_t[i] = scale2 * imagePoints_t[i];
objectPoints_t[i] = scale3 * objectPoints_t[i];
// std::cout << "imagePoints_t[i] = " << imagePoints_t[i] << std::endl;
// std::cout << "objectPoints_t[i] = " << objectPoints_t[i] << std::endl;
}
// std::cout<<avg3<<std::endl;
/* *******************************************************************************
* Similarity transformation T1 to normalize the image points,
* and a second similarity transformation T2 to normalize the space points.
* Page 181 in Multiple_View_Geometry_in_Computer_Vision__2nd_Edition
*/
Eigen::Matrix3d T1;
T1 << scale2, 0, -scale2 * avg2(0), 0, scale2, -scale2 * avg2(1), 0, 0, 1;
Eigen::MatrixXd T2(4, 4);
T2 << scale3, 0, 0, -scale3 * avg3(0), 0, scale3, 0, -scale3 * avg3(1), 0, 0, scale3, -scale3 * avg3(2), 0, 0, 0, 1;
// use Eigen
Eigen::MatrixXd A(2 * n, 12);
A.setZero(2 * n, 12);
for (int i = 0; i < n; i++)
{
A(2 * i, 0) = objectPoints_t[i](0);
A(2 * i, 1) = objectPoints_t[i](1);
A(2 * i, 2) = objectPoints_t[i](2);
A(2 * i, 3) = 1;
A(2 * i, 4) = 0;
A(2 * i, 5) = 0;
A(2 * i, 6) = 0;
A(2 * i, 7) = 0;
A(2 * i, 8) = -imagePoints_t[i](0) * objectPoints_t[i](0);
A(2 * i, 9) = -imagePoints_t[i](0) * objectPoints_t[i](1);
A(2 * i, 10) = -imagePoints_t[i](0) * objectPoints_t[i](2);
A(2 * i, 11) = -imagePoints_t[i](0) * 1;
A(2 * i + 1, 0) = 0;
A(2 * i + 1, 1) = 0;
A(2 * i + 1, 2) = 0;
A(2 * i + 1, 3) = 0;
A(2 * i + 1, 4) = 1 * objectPoints_t[i](0);
A(2 * i + 1, 5) = 1 * objectPoints_t[i](1);
A(2 * i + 1, 6) = 1 * objectPoints_t[i](2);
A(2 * i + 1, 7) = 1;
A(2 * i + 1, 8) = -imagePoints_t[i](1) * objectPoints_t[i](0);
A(2 * i + 1, 9) = -imagePoints_t[i](1) * objectPoints_t[i](1);
A(2 * i + 1, 10) = -imagePoints_t[i](1) * objectPoints_t[i](2);
A(2 * i + 1, 11) = -imagePoints_t[i](1) * 1;
}
Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV);
const Eigen::VectorXd & v1 = svd.matrixV().col(11);
this->Pt << v1(0), v1(1), v1(2), v1(3), v1(4), v1(5), v1(6), v1(7), v1(8), v1(9), v1(10), v1(11);
// std::cout<<"A= \n"<< A<<std::endl;
// std::cout<<Pt<<std::endl;
P = T1.inverse() * Pt * T2;
//Decompose the projection matrix
cv::Mat Pr(3, 4, cv::DataType<float>::type);
cv::Mat Mt(3, 3, cv::DataType<float>::type);
cv::Mat Rt(3, 3, cv::DataType<float>::type);
cv::Mat Tt(4, 1, cv::DataType<float>::type);
for (unsigned i = 0; i < 3; i++)
{
for (unsigned int j = 0; j < 4; j++)
{
Pr.at<float> (i, j) = P(i, j);
}
}
cv::decomposeProjectionMatrix(Pr, Mt, Rt, Tt);
//scale: Mt(2,2) should = 1; so update the projection matrix and decomposition again
float k = (1 / (Mt.at<float> (2, 2)));
/* ****************************************************************************************
* Upate the projection matrix
* Decomposition again to get new intrinsic matrix and rotation matrix
*/
this->P = k * P;
cv::Mat Pro(3, 4, cv::DataType<float>::type);
cv::Mat Mc(3, 3, cv::DataType<float>::type); // intrinsic parameter matrix
cv::Mat Rc(3, 3, cv::DataType<float>::type); // rotation matrix
cv::Mat Tc(4, 1, cv::DataType<float>::type); // translation vector
for (unsigned i = 0; i < 3; i++)
{
for (unsigned int j = 0; j < 4; j++)
{
Pro.at<float> (i, j) = P(i, j);
}
}
cv::decomposeProjectionMatrix(Pro, Mc, Rc, Tc);
// Change from OpenCV varibles to Eigen
for (unsigned i = 0; i < 3; i++)
{
for (unsigned int j = 0; j < 3; j++)
{
M(i, j) = Mc.at<float> (i, j) ;
}
}
/* ****************************************************************************************
* Compute te rotation matrix R and translation vector T
*/
P_temp = M.inverse() * P;
this->R = this->P_temp.block(0,0,3,3);
this->T = this->P_temp.block(0,3,3,1);
std::cout << "+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n";
// std::cout << "T1 =\n " << T2 << std::endl;
// std::cout << "T2 =\n " << T1 << std::endl;
// std::cout << "A =\n " << A << std::endl;
// std::cout << "svd.matrixV() =\n " << svd.matrixV() << std::endl;
// std::cout << "Pt =\n " << Pt << std::endl;
std::cout << "P =\n " << P << std::endl;
std::cout << "M =\n " << M << std::endl;
std::cout << "R =\n " << R << std::endl;
std::cout << "Rc =\n " << Rc << std::endl;
std::cout << "T =\n " << T << std::endl;
std::cout << "Mt(2,2) = " << Mt.at<float> (2, 2) << std::endl;
}
