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