int
main (void)
{
testA ();
testB ();
testC ();
testD ();
testE ();
testF ();
testG ();
testH ();
testI ();
testJ ();
testK ();
testL ();
testM ();
testN ();
testO ();
testP ();
testQ ();
testR ();
testS ();
testT ();
testU ();
testV ();
/*
testW ();
testX ();
testY ();
testZ ();
*/
exit (0);
}
bool ManifoldManager::isRegular(Delaunay3::Vertex_handle &v) {
std::vector<Delaunay3::Cell_handle> incidentCells;
std::vector<Delaunay3::Vertex_handle> incidentV;
dt_.incident_cells(v, std::back_inserter(incidentCells));
dt_.adjacent_vertices(v, std::back_inserter(incidentV));
std::vector<Delaunay3::Vertex_handle> pathVert;
std::vector<Delaunay3::Edge> pathEdge;
std::vector<Delaunay3::Edge> pathEdgeUnique;
/*look for mesh edges concurring in each vertex (incidentV) incident to v*/
for (auto incV : incidentV) {
/*Find one tetrahedron with edge v->incV*/
auto c = incidentCells.begin();
bool found = false;
Delaunay3::Edge curEdge;
Delaunay3::Cell_handle startingCell;
while (c != incidentCells.end() && !found) {
if ((*c)->has_vertex(incV)) {
Delaunay3::Edge e((*c), (*c)->index(incV), (*c)->index(v));
curEdge = e;
found = true;
startingCell = (*c);
}
c++;
}
if (found) {
Delaunay3::Cell_circulator cellCirc = dt_.incident_cells(curEdge, startingCell);
Delaunay3::Cell_handle lastCell = cellCirc;
bool lastManif = cellCirc->info().iskeptManifold();
/*look for edges of the mesh*/
do {
cellCirc++;
if (lastManif == !cellCirc->info().iskeptManifold()) {
int curIdx;
for (int curV = 0; curV < 4; ++curV) {/*look for the vertex along the mesh elements of the edge starting from incV*/
Delaunay3::Vertex_handle curVonLast = cellCirc->vertex(curV);
if (curV != cellCirc->index(v) && curV != cellCirc->index(incV) && lastCell->has_vertex(curVonLast)) {
curIdx = curV;
}
}
/*store the edge of the mesh*/
pathEdge.push_back(Delaunay3::Edge(cellCirc, cellCirc->index(incV), curIdx));
}
lastManif = cellCirc->info().iskeptManifold();
lastCell = cellCirc;
} while (cellCirc != startingCell);
}
}
std::vector<bool> testE(pathEdge.size(), true);
for (int idxExt = 0; idxExt < pathEdge.size(); ++idxExt) {
if (testE[idxExt]) {
Delaunay3::Vertex_handle vE1 = pathEdge[idxExt].first->vertex(pathEdge[idxExt].second);
Delaunay3::Vertex_handle vE2 = pathEdge[idxExt].first->vertex(pathEdge[idxExt].third);
pathVert.push_back(vE1);
pathVert.push_back(vE2);
pathEdgeUnique.push_back(pathEdge[idxExt]);
//Remove duplicates
for (int idxInt = idxExt + 1; idxInt < pathEdge.size(); ++idxInt) {
Delaunay3::Vertex_handle vI1 = pathEdge[idxInt].first->vertex(pathEdge[idxInt].second);
Delaunay3::Vertex_handle vI2 = pathEdge[idxInt].first->vertex(pathEdge[idxInt].third);
if ((vI1 == vE1 && vI2 == vE2) || (vI1 == vE2 && vI2 == vE1)) {
testE[idxInt] = false;
}
}
}
}
if (pathEdgeUnique.empty()) {
return true;
}
std::vector<bool> yetVisited(pathEdgeUnique.size(), false);
int curEdgeIdx = 0;
Delaunay3::Vertex_handle initV = pathEdgeUnique[0].first->vertex(pathEdgeUnique[0].second);
Delaunay3::Vertex_handle curV = pathEdgeUnique[0].first->vertex(pathEdgeUnique[0].second);
//yetVisited[curEdgeIdx] = true;
do {
int countConcurr = 0;
int testEdgeOK = 0;
for (int testEdge = 0; testEdge < pathEdgeUnique.size(); testEdge++) {
if (curV == pathEdgeUnique[testEdge].first->vertex(pathEdgeUnique[testEdge].second)
|| curV == pathEdgeUnique[testEdge].first->vertex(pathEdgeUnique[testEdge].third)) {
countConcurr++;
if (yetVisited[testEdge] == false) {
testEdgeOK = testEdge;
}
}
}
if (countConcurr != 2) {
return false;
}
// std::cout << "Step2:(" << curV->point().x() << ", " << curV->point().y() << "," << curV->point().z() << ") " << std::endl;
if (curV == pathEdgeUnique[testEdgeOK].first->vertex(pathEdgeUnique[testEdgeOK].second)) {
curV = pathEdgeUnique[testEdgeOK].first->vertex(pathEdgeUnique[testEdgeOK].third);
} else if (curV == pathEdgeUnique[testEdgeOK].first->vertex(pathEdgeUnique[testEdgeOK].third)) {
curV = pathEdgeUnique[testEdgeOK].first->vertex(pathEdgeUnique[testEdgeOK].second);
} else {
//std::cerr << "isRegular SOMETHING WRONG" << std::endl;
}
//std::cout << "Step3:(" << curV->point().x() << ", " << curV->point().y() << "," << curV->point().z() << ") " << std::endl;
yetVisited[testEdgeOK] = true;
} while (curV != initV);
for (auto v : yetVisited) {
if (!v) {
return false;
}
}
return true;
}
/** 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 MonoOdometer5::estimateMotion(std::vector<Match> matches, Eigen::Matrix3f &R, Eigen::Vector3f &t, bool showOpticalFlow, std::vector<Match> &inlierMatches, std::vector<Eigen::Vector3f> &points3D)
{
counter_total++;
// check number of correspondences
int N = matches.size();
if(N < param_odometerMinNumberMatches_)
{
// too few matches to compute F
std::cout << "------------ NOT ENOUGH MATCHES" << std::endl;
R = Eigen::Matrix3f::Identity();
t << 0.0, 0.0, 0.0;
std::cout << "SINGULARITIES / TOTAL: " << counter_sing << " / " << counter_total << std::endl;
return false;
}
std::vector<cv::Point2f> prevPoints = getPrevPoints(matches);
std::vector<cv::Point2f> currPoints = getCurrPoints(matches);
cv::Mat E, Rcv, tcv, mask;
double focal = K_(0, 0);
cv::Point2f pp(K_(0, 2), K_(1, 2));
E = cv::findEssentialMat(currPoints, prevPoints, focal, pp, cv::RANSAC, 0.999, 1.0, mask);
if(!testE(E))
{
// too few matches to compute F
std::cout << "------------ SINGULARITY IN ESSENTIAL MATRIX" << std::endl;
counter_sing++;
R = Eigen::Matrix3f::Identity();
t << 0.0, 0.0, 0.0;
std::cout << "SINGULARITIES / TOTAL: " << counter_sing << " / " << counter_total << std::endl;
return false;
}
cv::recoverPose(E, currPoints, prevPoints, Rcv, tcv, focal, pp, mask);
R << Rcv.at<double>(0,0), Rcv.at<double>(0,1), Rcv.at<double>(0,2),
Rcv.at<double>(1,0), Rcv.at<double>(1,1), Rcv.at<double>(1,2),
Rcv.at<double>(2,0), Rcv.at<double>(2,1), Rcv.at<double>(2,2);
t << tcv.at<double>(0,0), tcv.at<double>(1,0), tcv.at<double>(2,0);
// save inlier and outlier matches
std::vector<Match> outlierMatches;
for(int i=0; i<matches.size(); i++)
{
if(mask.at<char>(i, 0) == 1)
{
inlierMatches.push_back(matches[i]);
}
else
{
outlierMatches.push_back(matches[i]);
}
}
if(showOpticalFlow)
{
//std::cout << "R" << std::endl << R << std::endl;
//std::cout << "t" << std::endl << t << std::endl;
cv::Mat image(1024, 768, CV_8UC3, cv::Scalar(0,0,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", of1);
cv::waitKey(10);
}
//std::cout << "SINGULARITIES / TOTAL: " << counter_sing << " / " << counter_total << std::endl;
return true;
}
int main(){
testE();
system("PAUSE");
return 0;
}