ON_2dPoint
FittingSurface::inverseMapping (const ON_NurbsSurface &nurbs, const ON_3dPoint &pt, const ON_2dPoint &hint, double &error,
ON_3dPoint &p, ON_3dVector &tu, ON_3dVector &tv, int maxSteps, double accuracy, bool quiet)
{
double pointAndTangents[9];
ON_2dVector current, delta;
ON_Matrix A(2,2);
ON_2dVector b;
ON_3dVector r;
std::vector<double> elementsU = getElementVector (nurbs, 0);
std::vector<double> elementsV = getElementVector (nurbs, 1);
double minU = elementsU[0];
double minV = elementsV[0];
double maxU = elementsU[elementsU.size () - 1];
double maxV = elementsV[elementsV.size () - 1];
current = hint;
for (int k = 0; k < maxSteps; k++)
{
nurbs.Evaluate (current[0], current[1], 1, 3, pointAndTangents);
p[0] = pointAndTangents[0];
p[1] = pointAndTangents[1];
p[2] = pointAndTangents[2];
tu[0] = pointAndTangents[3];
tu[1] = pointAndTangents[4];
tu[2] = pointAndTangents[5];
tv[0] = pointAndTangents[6];
tv[1] = pointAndTangents[7];
tv[2] = pointAndTangents[8];
r = p - pt;
b[0] = -ON_DotProduct(r,tu);
b[1] = -ON_DotProduct(r,tv);
A[0][0] = ON_DotProduct(tu,tu);
A[0][1] = ON_DotProduct(tu,tv);
A[1][0] = A[0][1];
A[1][1] = ON_DotProduct(tv,tv);
Eigen::Vector2d eb(b[0],b[1]);
Eigen::Vector2d edelta;
Eigen::Matrix2d eA;
eA (0, 0) = A[0][0];
eA (0, 1) = A[0][1];
eA (1, 0) = A[1][0];
eA (1, 1) = A[1][1];
edelta = eA.ldlt ().solve (eb);
delta[0] = edelta (0);
delta[1] = edelta (1);
if (sqrt(ON_DotProduct(delta,delta)) < accuracy)
{
error = sqrt(ON_DotProduct(r,r));
return current;
}
else
{
current = current + delta;
if (current[0] < minU)
current[0] = minU;
else if (current[0] > maxU)
current[0] = maxU;
if (current[1] < minV)
current[1] = minV;
else if (current[1] > maxV)
current[1] = maxV;
}
}
error = sqrt(ON_DotProduct(r,r));
if (!quiet)
{
printf ("[FittingSurface::inverseMapping] Warning: Method did not converge (%e %d)\n", accuracy, maxSteps);
printf (" %f %f ... %f %f\n", hint[0], hint[1], current[0], current[1]);
}
return current;
}
void ProbabilisticStereoTriangulator<CAMERA_GEOMETRY_T>::getUncertainty(
size_t keypointIdxA, size_t keypointIdxB,
const Eigen::Vector4d& homogeneousPoint_A,
Eigen::Matrix3d& outPointUOplus_A, bool& outCanBeInitialized) const {
OKVIS_ASSERT_TRUE_DBG(Exception,frameA_&&frameB_,"initialize with frames before use!");
// also get the point in the other coordinate representation
//Eigen::Vector4d& homogeneousPoint_B=_T_BA*homogeneousPoint_A;
Eigen::Vector4d hPA = homogeneousPoint_A;
// calculate point uncertainty by constructing the lhs of the Gauss-Newton equation system.
// note: the transformation T_WA is assumed constant and identity w.l.o.g.
Eigen::Matrix<double, 9, 9> H = H_;
// keypointA_t& kptA = _frameA_ptr->keypoint(keypointIdxA);
// keypointB_t& kptB = _frameB_ptr->keypoint(keypointIdxB);
Eigen::Vector2d kptA, kptB;
frameA_->getKeypoint(camIdA_, keypointIdxA, kptA);
frameB_->getKeypoint(camIdB_, keypointIdxB, kptB);
// assemble the stuff from the reprojection errors
double keypointStdDev;
frameA_->getKeypointSize(camIdA_, keypointIdxA, keypointStdDev);
keypointStdDev = 0.8 * keypointStdDev / 12.0;
Eigen::Matrix2d inverseMeasurementCovariance = Eigen::Matrix2d::Identity()
* (1.0 / (keypointStdDev * keypointStdDev));
::okvis::ceres::ReprojectionError<CAMERA_GEOMETRY_T> reprojectionErrorA(
frameA_->geometryAs<CAMERA_GEOMETRY_T>(camIdA_), 0, kptA,
inverseMeasurementCovariance);
//typename keypointA_t::measurement_t residualA;
Eigen::Matrix<double, 2, 1> residualA;
Eigen::Matrix<double, 2, 4, Eigen::RowMajor> J_hpA;
Eigen::Matrix<double, 2, 3, Eigen::RowMajor> J_hpA_min;
double* jacobiansA[3];
jacobiansA[0] = 0; // do not calculate, T_WA is fixed identity transform
jacobiansA[1] = J_hpA.data();
jacobiansA[2] = 0; // fixed extrinsics
double* jacobiansA_min[3];
jacobiansA_min[0] = 0; // do not calculate, T_WA is fixed identity transform
jacobiansA_min[1] = J_hpA_min.data();
jacobiansA_min[2] = 0; // fixed extrinsics
const double* parametersA[3];
//const double* test = _poseA.parameters();
parametersA[0] = poseA_.parameters();
parametersA[1] = hPA.data();
parametersA[2] = extrinsics_.parameters();
reprojectionErrorA.EvaluateWithMinimalJacobians(parametersA, residualA.data(),
jacobiansA, jacobiansA_min);
inverseMeasurementCovariance.setIdentity();
frameB_->getKeypointSize(camIdB_, keypointIdxB, keypointStdDev);
keypointStdDev = 0.8 * keypointStdDev / 12.0;
inverseMeasurementCovariance *= 1.0 / (keypointStdDev * keypointStdDev);
::okvis::ceres::ReprojectionError<CAMERA_GEOMETRY_T> reprojectionErrorB(
frameB_->geometryAs<CAMERA_GEOMETRY_T>(camIdB_), 0, kptB,
inverseMeasurementCovariance);
Eigen::Matrix<double, 2, 1> residualB;
Eigen::Matrix<double, 2, 7, Eigen::RowMajor> J_TB;
Eigen::Matrix<double, 2, 6, Eigen::RowMajor> J_TB_min;
Eigen::Matrix<double, 2, 4, Eigen::RowMajor> J_hpB;
Eigen::Matrix<double, 2, 3, Eigen::RowMajor> J_hpB_min;
double* jacobiansB[3];
jacobiansB[0] = J_TB.data();
jacobiansB[1] = J_hpB.data();
jacobiansB[2] = 0; // fixed extrinsics
double* jacobiansB_min[3];
jacobiansB_min[0] = J_TB_min.data();
jacobiansB_min[1] = J_hpB_min.data();
jacobiansB_min[2] = 0; // fixed extrinsics
const double* parametersB[3];
parametersB[0] = poseB_.parameters();
parametersB[1] = hPA.data();
parametersB[2] = extrinsics_.parameters();
reprojectionErrorB.EvaluateWithMinimalJacobians(parametersB, residualB.data(),
jacobiansB, jacobiansB_min);
// evaluate again closer:
hPA.head<3>() = 0.8 * (hPA.head<3>() - T_AB_.r() / 2.0 * hPA[3])
+ T_AB_.r() / 2.0 * hPA[3];
reprojectionErrorB.EvaluateWithMinimalJacobians(parametersB, residualB.data(),
jacobiansB, jacobiansB_min);
if (residualB.transpose() * residualB < 4.0)
outCanBeInitialized = false;
else
outCanBeInitialized = true;
// now add to H:
H.bottomRightCorner<3, 3>() += J_hpA_min.transpose() * J_hpA_min;
H.topLeftCorner<6, 6>() += J_TB_min.transpose() * J_TB_min;
H.topRightCorner<6, 3>() += J_TB_min.transpose() * J_hpB_min;
H.bottomLeftCorner<3, 6>() += J_hpB_min.transpose() * J_TB_min;
H.bottomRightCorner<3, 3>() += J_hpB_min.transpose() * J_hpB_min;
// invert (if invertible) to get covariance:
Eigen::Matrix<double, 9, 9> cov;
if (H.colPivHouseholderQr().rank() < 9) {
outCanBeInitialized = false;
return;
}
cov = H.inverse(); // FIXME: use the QR decomposition for this...
outPointUOplus_A = cov.bottomRightCorner<3, 3>();
}
void Triangle::setJac(){
Eigen::Matrix2d J;
J.col(0) = (M_points[1]-M_points[0]).toEig();
J.col(1) = (M_points[2]-M_points[0]).toEig();
M_jac = J;
}
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;
}
}
}
