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; } } }