void triangulate(const Eigen::Vector3d & point1, const Eigen::Vector3d & ray1,
const Eigen::Vector3d & point2, const Eigen::Vector3d & ray2,
Eigen::Vector3d & outTriangulatedPoint, double & outGap,
double & outS1, double & outS2) {
Eigen::Vector3d t12 = point2 - point1;
Eigen::Vector2d b;
b[0] = t12.dot(ray1);
b[1] = t12.dot(ray2);
Eigen::Matrix2d A;
A(0, 0) = ray1.dot(ray1);
A(1, 0) = ray1.dot(ray2);
A(0, 1) = -A(1, 0);
A(1, 1) = -ray2.dot(ray2);
Eigen::Vector2d lambda = A.inverse() * b;
Eigen::Vector3d xm = point1 + lambda[0] * ray1;
Eigen::Vector3d xn = point2 + lambda[1] * ray2;
t12 = (xm - xn);
outGap = t12.norm();
outTriangulatedPoint = xn + 0.5 * t12;
outS1 = lambda[0];
outS2 = lambda[1];
}
void GreenStrain_LIMSolver2D::prepareProblemData(std::vector<int>& hessRowIdx, std::vector<int>& hessColIdx)
{
// Compute deformation gradients
int numTets = mesh->Triangles->rows();
Ms.resize(3,2*numTets);
MMTs.resize(3,3*numTets);
Eigen::Matrix<double,3,2> SelectorM;
SelectorM.block<2,2>(0,0) = Eigen::Matrix<double,2,2>::Identity();
SelectorM.row(2) = Eigen::Vector2d::Ones()*-1;
for(int t=0;t<numTets;t++)
{
Eigen::Vector2d A,B,C;
if(mesh->IsCorotatedTriangles)
{
A = mesh->CorotatedTriangles->row(t).block<1,2>(0,0).cast<double>();
B = mesh->CorotatedTriangles->row(t).block<1,2>(0,2).cast<double>();
C = mesh->CorotatedTriangles->row(t).block<1,2>(0,4).cast<double>();
}
else
{
A = mesh->InitalVertices->row(mesh->Triangles->coeff(t,0)).block<1,2>(0,0).cast<double>();
B = mesh->InitalVertices->row(mesh->Triangles->coeff(t,1)).block<1,2>(0,0).cast<double>();
C = mesh->InitalVertices->row(mesh->Triangles->coeff(t,2)).block<1,2>(0,0).cast<double>();
}
Eigen::Matrix2d V;
V << A-C,B-C;
Eigen::Matrix<double,3,2> Mtemp = SelectorM*V.inverse().cast<double>();
Ms.block<3,2>(0,2*t) = Mtemp;
MMTs.block<3,3>(0,3*t) = Mtemp*Mtemp.transpose();
}
}
bool FastConvexFitting::getEvaluation(Geometry & geometry)
{
QVector<int> edgeid;
QVector<int> startid;
QVector<int> endid;
if(getBeamRange(geometry,edgeid,startid,endid))
{
geometry.score=1;
int j,m=edgeid.size();
for(j=0;j<m;j++)
{
if(startid[j]>endid[j])
{
endid[j]+=beamsnum;
}
Eigen::Matrix2d A;
A.block(0,0,2,1)=orientation*(geometry.edges[edgeid[j]].startcorner-geometry.edges[edgeid[j]].endcorner);
Eigen::Vector2d B;
B=position+orientation*(geometry.edges[edgeid[j]].startcorner);
int k;
int count=0;
double score=1;
for(k=startid[j];k<=endid[j];k++)
{
int tmpid=k;
if(tmpid>=beamsnum)
{
tmpid-=beamsnum;
}
if(beams[tmpid]<=MINIMUMDISTANCE)
{
continue;
}
A.block(0,1,2,1)=points[tmpid];
Eigen::Vector2d x=A.inverse()*B;
score*=getGain(x(0),x(1),beams[tmpid]);
count++;
}
if(count>0)
{
geometry.score*=pow(score,1.0/double(count));
}
else
{
geometry.score=0;
return 0;
}
}
return 1;
}
else
{
geometry.score=0;
return 0;
}
}
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;
}
void RadialTangentialDistortion::undistort(
const Eigen::MatrixBase<DERIVED> & yconst,
const Eigen::MatrixBase<DERIVED_JY> & outJy) const {
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE_OR_DYNAMIC(
Eigen::MatrixBase<DERIVED>, 2);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE_OR_DYNAMIC(
Eigen::MatrixBase<DERIVED_JY>, 2, 2);
Eigen::MatrixBase<DERIVED> & y =
const_cast<Eigen::MatrixBase<DERIVED> &>(yconst);
y.derived().resize(2);
// we use f^-1 ' = ( f'(f^-1) ) '
// with f^-1 the undistortion
// and f the distortion
undistort(y); // first get the undistorted image
Eigen::Vector2d kp = y;
Eigen::Matrix2d Jd;
distort(kp, Jd);
// now y = f^-1(y0)
DERIVED_JY & J = const_cast<DERIVED_JY &>(outJy.derived());
J = Jd.inverse();
/* std::cout << "J: " << std::endl << J << std::endl;
double mx2_u = y[0]*y[0];
double my2_u = y[1]*y[1];
//double mxy_u = y[0]*y[1];
double rho2_u = mx2_u+my2_u;
double rad_dist_u = _k1*rho2_u+_k2*rho2_u*rho2_u;
// take the inverse as Jacobian.
J(0,0) = 1/(1 + rad_dist_u + _k1*2*mx2_u + _k2*rho2_u*4*mx2_u + 2*_p1*y[1] + 6*_p2*y[0]);
J(1,0) = (_k1*2*y[0]*y[1] + _k2*4*rho2_u*y[0]*y[1] + _p1*2*y[0] + 2*_p2*y[1]);
J(0,1) = J(1,0);
J(1,1) = 1/(1 + rad_dist_u + _k1*2*my2_u + _k2*rho2_u*4*my2_u + 6*_p1*y[1] + 2*_p2*y[0]);
// this should only happen if the distortion coefficients are 0
// the coefficients being zero removes the cross dependence then it is safe to set J(1,0) = 0
if (J(1,0) != 0)
J(1,0) = 1/J(1,0);
J(0,1) = J(1,0);*/
}
mathtools::geometry::euclidian::HyperEllipse<2> skeleton::model::Perspective::toObj(
const Eigen::Matrix<double,skeleton::model::meta<skeleton::model::Projective>::stordim,1> &vec,
const mathtools::geometry::euclidian::HyperEllipse<2> &) const
{
double z = 1.0/(sqrt(vec(0)*vec(0) + vec(1)*vec(1) + 1.0 - vec(2)*vec(2)));
double x = vec(0) * z;
double y = vec(1) * z;
Eigen::Matrix2d mat;
mat << 1.0-x*x , -x*y ,
-x*y , 1.0-y*y ;
Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> solv(mat);
Eigen::Matrix2d axes = Eigen::Matrix2d::Identity();
double fac = Eigen::Vector2d(-x*z,-y*z).transpose()*mat.inverse()*Eigen::Vector2d(-x*z,-y*z) - 1.0 + z*z;
axes = solv.eigenvectors()*Eigen::DiagonalMatrix<double,2>(Eigen::Vector2d(sqrt(fac/solv.eigenvalues()(0)),sqrt(fac/solv.eigenvalues()(1))));
mathtools::affine::Point<2> pt(vec(0),vec(1),m_frame2);
return mathtools::geometry::euclidian::HyperEllipse<2>(pt,m_frame2->getBasis()->getMatrix()*axes);
}
void EKF::correct(Eigen::Vector2d z_i)
{
// measurement matrix
Eigen::Matrix2d I = Eigen::Matrix2d::Identity();
Eigen::Matrix2d H = Eigen::Matrix2d::Identity();
// innovation residual
Eigen::Vector2d y = z_i - H * this->x;
// innovation covariance
Eigen::Matrix2d S = H * this->P * H.transpose() + this->R;
// Kalman gain
Eigen::Matrix2d K = this->P * H.transpose() * S.inverse();
// updated state
this->x = this->x + K * y;
// updated covariance
this->P = (I - K * H) * this->P;
}
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;
}
}
}
