double volume(OpenMesh::Vec3f& pointA, OpenMesh::Vec3f& pointB, OpenMesh::Vec3f& pointC)
{
// http://www.ditutor.com/vectors/volume_tetrahedron.html
Mat3 m = pointsToMat(pointA , pointB, pointC);
// use minus sign or change the order of the points in above function call
return -m.determinant()/6.0;
}
// Check the properties of a fundamental matrix:
//
// 1. The determinant is 0 (rank deficient)
// 2. The condition x'T*F*x = 0 is satisfied to precision.
//
bool ExpectFundamentalProperties(const Mat3 &F,
const Mat &ptsA,
const Mat &ptsB,
double precision) {
bool bOk = true;
bOk &= F.determinant() < precision;
assert(ptsA.cols() == ptsB.cols());
const Mat hptsA = ptsA.colwise().homogeneous();
const Mat hptsB = ptsB.colwise().homogeneous();
for (int i = 0; i < ptsA.cols(); ++i) {
const double residual = hptsB.col(i).dot(F * hptsA.col(i));
bOk &= residual < precision;
}
return bOk;
}
IGL_INLINE void igl::fit_rotations(
const Eigen::PlainObjectBase<DerivedS> & S,
const bool single_precision,
Eigen::PlainObjectBase<DerivedD> & R)
{
using namespace std;
const int dim = S.cols();
const int nr = S.rows()/dim;
assert(nr * dim == S.rows());
assert(dim == 3);
// resize output
R.resize(dim,dim*nr); // hopefully no op (should be already allocated)
//std::cout<<"S=["<<std::endl<<S<<std::endl<<"];"<<std::endl;
//MatrixXd si(dim,dim);
Eigen::Matrix<typename DerivedS::Scalar,3,3> si;// = Eigen::Matrix3d::Identity();
// loop over number of rotations we're computing
for(int r = 0;r<nr;r++)
{
// build this covariance matrix
for(int i = 0;i<dim;i++)
{
for(int j = 0;j<dim;j++)
{
si(i,j) = S(i*nr+r,j);
}
}
typedef Eigen::Matrix<typename DerivedD::Scalar,3,3> Mat3;
typedef Eigen::Matrix<typename DerivedD::Scalar,3,1> Vec3;
Mat3 ri;
if(single_precision)
{
polar_svd3x3(si, ri);
}else
{
Mat3 ti,ui,vi;
Vec3 _;
igl::polar_svd(si,ri,ti,ui,_,vi);
}
assert(ri.determinant() >= 0);
R.block(0,r*dim,dim,dim) = ri.block(0,0,dim,dim).transpose();
//cout<<matlab_format(si,C_STR("si_"<<r))<<endl;
//cout<<matlab_format(ri.transpose().eval(),C_STR("ri_"<<r))<<endl;
}
}
void KRt_From_P(const Mat34 &P, Mat3 *Kp, Mat3 *Rp, Vec3 *tp) {
// Decompose using the RQ decomposition HZ A4.1.1 pag.579.
Mat3 K = P.block(0, 0, 3, 3);
Mat3 Q;
Q.setIdentity();
// Set K(2,1) to zero.
if (K(2,1) != 0) {
double c = -K(2,2);
double s = K(2,1);
double l = sqrt(c * c + s * s);
c /= l; s /= l;
Mat3 Qx;
Qx << 1, 0, 0,
0, c, -s,
0, s, c;
K = K * Qx;
Q = Qx.transpose() * Q;
}
// Set K(2,0) to zero.
if (K(2,0) != 0) {
double c = K(2,2);
double s = K(2,0);
double l = sqrt(c * c + s * s);
c /= l; s /= l;
Mat3 Qy;
Qy << c, 0, s,
0, 1, 0,
-s, 0, c;
K = K * Qy;
Q = Qy.transpose() * Q;
}
// Set K(1,0) to zero.
if (K(1,0) != 0) {
double c = -K(1,1);
double s = K(1,0);
double l = sqrt(c * c + s * s);
c /= l; s /= l;
Mat3 Qz;
Qz << c,-s, 0,
s, c, 0,
0, 0, 1;
K = K * Qz;
Q = Qz.transpose() * Q;
}
Mat3 R = Q;
//Mat3 H = P.block(0, 0, 3, 3);
// RQ decomposition
//Eigen::HouseholderQR<Mat3> qr(H);
//Mat3 K = qr.matrixQR().triangularView<Eigen::Upper>();
//Mat3 R = qr.householderQ();
// Ensure that the diagonal is positive and R determinant == 1.
if (K(2,2) < 0) {
K = -K;
R = -R;
}
if (K(1,1) < 0) {
Mat3 S;
S << 1, 0, 0,
0,-1, 0,
0, 0, 1;
K = K * S;
R = S * R;
}
if (K(0,0) < 0) {
Mat3 S;
S << -1, 0, 0,
0, 1, 0,
0, 0, 1;
K = K * S;
R = S * R;
}
// Compute translation.
Eigen::PartialPivLU<Mat3> lu(K);
Vec3 t = lu.solve(P.col(3));
if(R.determinant()<0) {
R = -R;
t = -t;
}
// scale K so that K(2,2) = 1
K = K / K(2,2);
*Kp = K;
*Rp = R;
*tp = t;
}
void DecomposeProjectionMatrix(const Mat& P, Mat3* K, Mat3* R, Vec3* t)
{
// This is a modified version of the function "KRt_From_P", found in libmv and openMVG.
// It is subject to the terms of the Mozilla Public License, v. 2.0, a copy of the MPL
// can be found under "license.openMVG"
// Decompose using the RQ decomposition HZ A4.1.1 pag.579.
Mat3 Kp = P.block(0, 0, 3, 3);
Mat3 Q; Q.setIdentity();
// Set K(2, 1) to zero.
if (Kp(2, 1) != 0)
{
double c = -Kp(2, 2);
double s = Kp(2, 1);
double l = sqrt(c * c + s * s);
c /= l; s /= l;
Mat3 Qx;
Qx << 1, 0, 0,
0, c, -s,
0, s, c;
Kp = Kp * Qx;
Q = Qx.transpose() * Q;
}
// Set K(2, 0) to zero.
if (Kp(2, 0) != 0)
{
double c = Kp(2, 2);
double s = Kp(2, 0);
double l = sqrt(c * c + s * s);
c /= l; s /= l;
Mat3 Qy;
Qy << c, 0, s,
0, 1, 0,
-s, 0, c;
Kp = Kp * Qy;
Q = Qy.transpose() * Q;
}
// Set K(1, 0) to zero.
if (Kp(1, 0) != 0)
{
double c = -Kp(1, 1);
double s = Kp(1, 0);
double l = sqrt(c * c + s * s);
c /= l; s /= l;
Mat3 Qz;
Qz << c, -s, 0,
s, c, 0,
0, 0, 1;
Kp = Kp * Qz;
Q = Qz.transpose() * Q;
}
Mat3 Rp = Q;
// Ensure that the diagonal is positive and R determinant == 1
if (Kp(2, 2) < 0)
{
Kp = -Kp;
Rp = -Rp;
}
if (Kp(1, 1) < 0)
{
Mat3 S;
S << 1, 0, 0,
0, -1, 0,
0, 0, 1;
Kp = Kp * S;
Rp = S * Rp;
}
if (Kp(0, 0) < 0)
{
Mat3 S;
S << -1, 0, 0,
0, 1, 0,
0, 0, 1;
Kp = Kp * S;
Rp = S * Rp;
}
// Compute translation.
Vec3 tp = Kp.colPivHouseholderQr().solve(P.col(3));
if(Rp.determinant() < 0)
{
Rp = -Rp;
tp = -tp;
}
// Scale K so that K(2, 2) = 1
Kp = Kp / Kp(2, 2);
*K = Kp;
*R = Rp;
*t = tp;
}
bool FindExtrinsics(const Mat3& E, const Point2Vec& pts1, const Point2Vec& pts2, Mat3* R, Vec3* t)
{
const auto num_correspondences = pts1.size();
// Put first camera at origin
Mat3 R0; R0.setIdentity();
Vec3 t0; t0.setZero();
// Find the SVD of E
Eigen::JacobiSVD<Mat3> svd{E, Eigen::ComputeFullU | Eigen::ComputeFullV};
Mat3 U = svd.matrixU();
Mat3 Vt = svd.matrixV().transpose();
// Find R and t
Mat3 D;
D << 0, 1, 0,
-1, 0, 0,
0, 0, 1;
Mat3 Ra = U * D * Vt;
Mat3 Rb = U * D.transpose() * Vt;
if (Ra.determinant() < 0.0) Ra *= -1.0;
if (Rb.determinant() < 0.0) Rb *= -1.0;
Vec3 tu = U.col(2);
// Figure out which configuration is correct using the supplied points
int c1_pos = 0, c2_pos = 0, c1_neg = 0, c2_neg = 0;
for (std::size_t i = 0; i < num_correspondences; i++)
{
const Point3 Q = Triangulate(Observations{Observation{pts1[i], R0, t0}, Observation{pts2[i], Ra, tu}});
const Point3 PQ = (Ra * Q) + tu;
if (Q.z() > 0) c1_pos++;
else c1_neg++;
if (PQ.z() > 0) c2_pos++;
else c2_neg++;
}
if (c1_pos < c1_neg && c2_pos < c2_neg)
{
*R = Ra;
*t = tu;
} else if (c1_pos > c1_neg && c2_pos > c2_neg)
{
*R = Ra;
*t = -tu;
} else
{
// Triangulate again
c1_pos = c1_neg = c2_pos = c2_neg = 0;
for (std::size_t i = 0; i < num_correspondences; i++)
{
const Point3 Q = Triangulate(Observations{Observation{pts1[i], R0, t0}, Observation{pts2[i], Rb, tu}});
const Point3 PQ = (Rb * Q) + tu;
if (Q.z() > 0) c1_pos++;
else c1_neg++;
if (PQ.z() > 0) c2_pos++;
else c2_neg++;
}
if (c1_pos < c1_neg && c2_pos < c2_neg)
{
*R = Rb;
*t = tu;
} else if (c1_pos > c1_neg && c2_pos > c2_neg)
{
*R = Rb;
*t = -tu;
} else
{
std::cerr << "[FindExtrinsics] Error: no case found!";
return false;
}
}
return true;
}
