void igl::crouzeix_raviart_cotmatrix( const Eigen::MatrixBase<DerivedV> & V, const Eigen::MatrixBase<DerivedF> & F, const Eigen::MatrixBase<DerivedE> & E, const Eigen::MatrixBase<DerivedEMAP> & EMAP, Eigen::SparseMatrix<LT> & L) { // number of rows const int m = F.rows(); // Element simplex size const int ss = F.cols(); // Mesh should be edge-manifold assert(F.cols() != 3 || is_edge_manifold(F)); typedef Eigen::Matrix<LT,Eigen::Dynamic,Eigen::Dynamic> MatrixXS; MatrixXS C; cotmatrix_entries(V,F,C); Eigen::MatrixXi F2E(m,ss); { int k =0; for(int c = 0;c<ss;c++) { for(int f = 0;f<m;f++) { F2E(f,c) = k++; } } } // number of entries inserted per facet const int k = ss*(ss-1)*2; std::vector<Eigen::Triplet<LT> > LIJV;LIJV.reserve(k*m); Eigen::VectorXi LI(k),LJ(k),LV(k); // Compensation factor to match scales in matlab version double factor = 2.0; switch(ss) { default: assert(false && "unsupported simplex size"); case 3: factor = 4.0; LI<<0,1,2,1,2,0,0,1,2,1,2,0; LJ<<1,2,0,0,1,2,0,1,2,1,2,0; LV<<2,0,1,2,0,1,2,0,1,2,0,1; break; case 4: factor *= -1.0; LI<<0,3,3,3,1,2,1,0,1,2,2,0,0,3,3,3,1,2,1,0,1,2,2,0; LJ<<1,0,1,2,2,0,0,3,3,3,1,2,0,3,3,3,1,2,1,0,1,2,2,0; LV<<2,3,4,5,0,1,2,3,4,5,0,1,2,3,4,5,0,1,2,3,4,5,0,1; break; } for(int f=0;f<m;f++) { for(int c = 0;c<k;c++) { LIJV.emplace_back( EMAP(F2E(f,LI(c))), EMAP(F2E(f,LJ(c))), (c<(k/2)?-1.:1.) * factor *C(f,LV(c))); } } L.resize(E.rows(),E.rows()); L.setFromTriplets(LIJV.begin(),LIJV.end()); }
IGL_INLINE void igl::GeneralPolyVectorFieldFinder<DerivedV, DerivedF>::computek() { K.setZero(numE); // For every non-border edge for (unsigned eid=0; eid<numE; ++eid) { if (!isBorderEdge[eid]) { int fid0 = E2F(eid,0); int fid1 = E2F(eid,1); Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N0 = FN.row(fid0); Eigen::Matrix<typename DerivedV::Scalar, 1, 3> N1 = FN.row(fid1); // find common edge on triangle 0 and 1 int fid0_vc = -1; int fid1_vc = -1; for (unsigned i=0;i<3;++i) { if (F2E(fid0,i) == eid) fid0_vc = i; if (F2E(fid1,i) == eid) fid1_vc = i; } assert(fid0_vc != -1); assert(fid1_vc != -1); Eigen::Matrix<typename DerivedV::Scalar, 1, 3> common_edge = V.row(F(fid0,(fid0_vc+1)%3)) - V.row(F(fid0,fid0_vc)); common_edge.normalize(); // Map the two triangles in a new space where the common edge is the x axis and the N0 the z axis Eigen::Matrix<typename DerivedV::Scalar, 3, 3> P; Eigen::Matrix<typename DerivedV::Scalar, 1, 3> o = V.row(F(fid0,fid0_vc)); Eigen::Matrix<typename DerivedV::Scalar, 1, 3> tmp = -N0.cross(common_edge); P << common_edge, tmp, N0; // P.transposeInPlace(); Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V0; V0.row(0) = V.row(F(fid0,0)) -o; V0.row(1) = V.row(F(fid0,1)) -o; V0.row(2) = V.row(F(fid0,2)) -o; V0 = (P*V0.transpose()).transpose(); // assert(V0(0,2) < 1e-10); // assert(V0(1,2) < 1e-10); // assert(V0(2,2) < 1e-10); Eigen::Matrix<typename DerivedV::Scalar, 3, 3> V1; V1.row(0) = V.row(F(fid1,0)) -o; V1.row(1) = V.row(F(fid1,1)) -o; V1.row(2) = V.row(F(fid1,2)) -o; V1 = (P*V1.transpose()).transpose(); // assert(V1(fid1_vc,2) < 10e-10); // assert(V1((fid1_vc+1)%3,2) < 10e-10); // compute rotation R such that R * N1 = N0 // i.e. map both triangles to the same plane double alpha = -atan2(V1((fid1_vc+2)%3,2),V1((fid1_vc+2)%3,1)); Eigen::Matrix<typename DerivedV::Scalar, 3, 3> R; R << 1, 0, 0, 0, cos(alpha), -sin(alpha) , 0, sin(alpha), cos(alpha); V1 = (R*V1.transpose()).transpose(); // assert(V1(0,2) < 1e-10); // assert(V1(1,2) < 1e-10); // assert(V1(2,2) < 1e-10); // measure the angle between the reference frames // k_ij is the angle between the triangle on the left and the one on the right Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref0 = V0.row(1) - V0.row(0); Eigen::Matrix<typename DerivedV::Scalar, 1, 3> ref1 = V1.row(1) - V1.row(0); ref0.normalize(); ref1.normalize(); double ktemp = atan2(ref1(1),ref1(0)) - atan2(ref0(1),ref0(0)); // just to be sure, rotate ref0 using angle ktemp... Eigen::Matrix<typename DerivedV::Scalar, 2, 2> R2; R2 << cos(ktemp), -sin(ktemp), sin(ktemp), cos(ktemp); Eigen::Matrix<typename DerivedV::Scalar, 1, 2> tmp1 = R2*(ref0.head(2)).transpose(); // assert(tmp1(0) - ref1(0) < 1e-10); // assert(tmp1(1) - ref1(1) < 1e-10); K[eid] = ktemp; } } }
/** Estimate monocular visual odometry. * @param std::vector<Match> vector with matches * @param Eigen::Matrix3f& (output) estimated rotation matrix * @param Eigen::Vector3f& (output) estimated translation vector * @param bool show optical flow (true), don't show otherwise * @param std::vector<Match> output vector with all inlier matches * @param std::vector<Eigen::Vector3f> output vector with 3D points, triangulated from all inlier matches * @return bool true is motion successfully estimated, false otherwise */ bool MonoOdometer8::estimateMotion(std::vector<Match> matches, Eigen::Matrix3f &R, Eigen::Vector3f &t, bool showOpticalFlow, std::vector<Match> &inlierMatches, std::vector<Eigen::Vector3f> &points3D) { // check number of correspondences int N = matches.size(); if(N < param_odometerMinNumberMatches_) { // too few matches to compute F R = Eigen::Matrix3f::Identity(); t << 0.0, 0.0, 0.0; return false; } // normalize 2D features Eigen::Matrix3f NormTPrev, NormTCurr; std::vector<Match> matchesNorm = normalize2DPoints(matches, NormTPrev, NormTCurr); Eigen::Matrix3f F, E; std::vector<int> inlierIndices; // RANSAC loop for(int j=0; j<param_odometerRansacIters_; j++) { // get random sample std::vector<int> chosenIndices = getRandomSample(matchesNorm.size(), 8); // compute fundamental matrix F = getF(matchesNorm, chosenIndices); // get inliers std::vector<int> inlierIndicesCurr = getInliers(matchesNorm, F); if(inlierIndicesCurr.size() > inlierIndices.size()) { inlierIndices = inlierIndicesCurr; } } // check number of inliers if(inlierIndices.size() < param_odometerMinNumberMatches_) { R = Eigen::Matrix3f::Identity(); t << 0.0, 0.0, 0.0; return false; } // compute fundamental matrix out of all inliers F = getF(matchesNorm, inlierIndices); // save inlier and outlier matches std::vector<Match> outlierMatches; for(int i=0; i<matches.size(); i++) { if(elemInVec(inlierIndices, i)) { inlierMatches.push_back(matches[i]); } else { outlierMatches.push_back(matches[i]); } } // plot optical flow and print #inliers (for debugging) if(showOpticalFlow) { cv::Mat image(1024, 768, CV_8UC1, cv::Scalar(0)); cv::Mat of1 = highlightOpticalFlow(image, inlierMatches, cv::Scalar(0, 255, 0)); cv::Mat of2 = highlightOpticalFlow(of1, outlierMatches, cv::Scalar(0, 0, 255)); cv::namedWindow("Optical flow", CV_WINDOW_AUTOSIZE); cv::imshow("Optical flow", of2); cv::waitKey(10); } // denormalize F F = NormTCurr.transpose() * F * NormTPrev; // compute essential matrix E E = F2E(F); // get rotation and translation and triangulate points Eigen::Matrix<float, 4, Eigen::Dynamic> points3DMat; E2Rt(E, inlierMatches, R, t, points3DMat); // normalize 3D points (force last coordinate to 0) for(int j=0; j<points3DMat.cols(); j++) { Eigen::Vector3f pt = points3DMat.block<3, 1>(0, j); double lastCoord = points3DMat(3, j); pt = pt / lastCoord; if(pt(2) > 0) { points3D.push_back(pt); } else { // remove match if not a valid point inlierMatches.erase(inlierMatches.begin() + j); } } // check number of valid points if(points3D.size() < param_odometerMinNumberMatches_) { R = Eigen::Matrix3f::Identity(); t << 0.0, 0.0, 0.0; return false; } // inforce translation norm to 1 t = t / sqrt(t.dot(t)); return true; }