bool FeatureCoordinates::getDepthFromTriangulation(const FeatureCoordinates& other, const V3D& C2rC2C1, const QPD& qC2C1, FeatureDistance& d) {
Eigen::Matrix<double,3,2> B;
B << qC2C1.rotate(get_nor().getVec()), other.get_nor().getVec();
const Eigen::Matrix2d BtB = B.transpose() * B;
if(BtB.determinant() < 0.000001) { // Projection rays almost parallel.
return false;
}
const Eigen::Vector2d dv = - BtB.inverse() * B.transpose() * C2rC2C1;
d.setParameter(fabs(dv[0]));
return true;
}
double covToSDPixels(const Eigen::Matrix2d & S)
{
const double dProductOfEigenvectors = S.determinant();
if(IS_DEBUG) CHECK(dProductOfEigenvectors < 0, "cov matrix not positive definite");
return pow(dProductOfEigenvectors, 0.25); //Approx. sigma in pixels
}
void Deformable::projectPositionsCluster(std::vector<int> cluster, int cluster_indx)
{
int numVertices = cluster.size();
if (numVertices <= 1) return;
int i;//,j,k;
double beta_cluster =params.lbeta.at(cluster_indx);
// center of mass
Eigen::Vector2d cm, originalCm;
cm.setZero(); originalCm.setZero();
double mass = 0.0;
int indx;
for (i = 0; i < numVertices; i++) {
indx= cluster.at(i);
double m = mMasses(indx,0);
if (mFixed(indx,0)) m *= 100.0;
mass += m;
cm += mNewPos.row(indx) * m;
originalCm += mOriginalPos.row(indx) * m;
//std::cout<<"before: "<<mNewPos.row(indx)<<std::endl;
}
cm /= mass;
originalCm /= mass;
Eigen::Matrix2d Apq;
Eigen::Matrix2d Aqq;
Eigen::Vector2d p;
Eigen::Vector2d q;
Apq.setZero();
Aqq.setZero();
for (i = 0; i < numVertices; i++) {
indx= cluster.at(i);
p(0) = mNewPos(indx,0) - cm(0);
p(1) = mNewPos(indx,1) - cm(1);
q(0) = mOriginalPos(indx,0) - originalCm(0);
q(1) = mOriginalPos(indx,1) - originalCm(1);
double m = mMasses(indx,0);
Apq += m*p*q.transpose();
Aqq += m*q*q.transpose();
}
if (!params.allowFlip && Apq.determinant() < 0.0f)
{ // prevent from flipping
Apq(0,1) = -Apq(0,1);
Apq(1,1) = -Apq(1,1);
}
Eigen::Matrix2d R;
Eigen::JacobiSVD<Eigen::MatrixXd> svd(Apq, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::MatrixXd U = svd.matrixU();
Eigen::MatrixXd V = svd.matrixV();
R = U*V.transpose();
if (!params.quadraticMatch) { // --------- linear match
Eigen::Matrix2d A = Aqq;
A = Apq*A.inverse();
if (params.volumeConservation) {
double det = A.determinant();
if (det != 0.0) {
det = 1.0 / sqrt(fabs(det));
if (det > 2.0) det = 2.0;
A = A*det;
}
}
Eigen::Matrix2d T = R * (1.0 - beta_cluster) + A * beta_cluster;
std::cout<<"cluster's beta "<<beta_cluster<<std::endl;
for (i = 0; i < numVertices; i++) {
int indx= cluster.at(i);
indxCount.at(indx) +=1;
if (mFixed(indx)) continue;
q(0) = mOriginalPos(indx,0) - originalCm(0);
q(1) = mOriginalPos(indx,1) - originalCm(1);
Eigen::Vector2d Tq = T*q;
mGoalPos(indx,0) = Tq(0)+cm(0);
mGoalPos(indx,1) = Tq(1)+cm(1);
mNewPos.row(indx) += (mGoalPos.row(indx) - mNewPos.row(indx)) * params.alpha;
mGoalPos_sum.row(indx) += mNewPos.row(indx);
}
}
}
void Deformable::projectPositions()
{
if (mNumVertices <= 1) return;
int i;//,j,k;
// center of mass
Eigen::Vector2d cm, originalCm;
cm.setZero(); originalCm.setZero();
double mass = 0.0;
for (i = 0; i < mNumVertices; i++) {
double m = mMasses(i,0);
if (mFixed(i,0)) m *= 1000.0;
mass += m;
cm += mNewPos.row(i) * m;
originalCm += mOriginalPos.row(i) * m;
}
//std::cout<<std::endl;
cm /= mass;
originalCm /= mass;
Eigen::Matrix2d Apq;
Eigen::Matrix2d Aqq;
Eigen::Vector2d p;
Eigen::Vector2d q;
Apq.setZero();
Aqq.setZero();
for (i = 0; i < mNumVertices; i++) {
p(0) = mNewPos(i,0) - cm(0);
p(1) = mNewPos(i,1) - cm(1);
q(0) = mOriginalPos(i,0) - originalCm(0);
q(1) = mOriginalPos(i,1) - originalCm(1);
double m = mMasses(i,0);
Apq += m*p*q.transpose();
Aqq += m*q*q.transpose();
}
/*if (!params.allowFlip && Apq.determinant() < 0.0f) { // prevent from flipping
Apq(0,1) = -Apq(0,1);
Apq(1,1) = -Apq(1,1);
}*/
//std::cout<<Apq<<std::endl;
Eigen::Matrix2d R;
Eigen::JacobiSVD<Eigen::MatrixXd> svd(Apq, Eigen::ComputeThinU | Eigen::ComputeThinV);
Eigen::MatrixXd U = svd.matrixU();
Eigen::MatrixXd V = svd.matrixV();
R = U*V.transpose();
if (!params.quadraticMatch) { // --------- linear match
Eigen::Matrix2d A = Aqq;
Eigen::Matrix2d A_=A.inverse();
/*std::cout<<A_.row(0)<<std::endl;
std::cout<<A_.row(1)<<std::endl;*/
A = Apq*A_;
if (params.volumeConservation) {
double det = A.determinant();
if(det<0) det=-det;
if (det != 0.0) {
det = 1.0 / sqrt(fabs(det));
if (det > 2.0) det = 2.0;
A *= det;
}
}
float one_beta =1.0 - params.beta;
Eigen::Matrix2d T = R * one_beta;
A=A*params.beta;
T = T + A;
for (i = 0; i < mNumVertices; i++) {
if (mFixed(i)) continue;
q(0) = mOriginalPos(i,0) - originalCm(0);
q(1) = mOriginalPos(i,1) - originalCm(1);
Eigen::Vector2d Tq = T*q;
mGoalPos(i,0) = Tq(0)+cm(0);
mGoalPos(i,1) = Tq(1)+cm(1);
//mGoalPos.row(i)=(R * (1.0f - params.beta) + A * params.beta)*q+cm;
mNewPos.row(i) += (mGoalPos.row(i) - mNewPos.row(i)) * params.alpha;
//std::cout<<T.row(0)<<std::endl;
//std::cout<<T.row(1)<<std::endl;
}
}
}