Mat GetFundamentalMat(const vector<TrackedPoint> & trackedPoints, vector<TrackedPoint>* inliers, double ransac_max_dist, double ransac_p) { //vector<TrackedPoint> trackedPoints; //vector<matf> H_out; //NormalizePoints2(trackedPoints_, trackedPoints, H_out); const int method = FM_RANSAC; //const int method = FM_LMEDS; const double param1 = ransac_max_dist; const double param2 = ransac_p; #ifdef OPENCV_2_1 matf pts1(trackedPoints.size(), 2), pts2(trackedPoints.size(), 2); for (size_t i = 0; i < trackedPoints.size(); ++i) { pts1(i, 0) = trackedPoints[i].x1; pts2(i, 1) = trackedPoints[i].y1; pts2(i, 0) = trackedPoints[i].x2; pts2(i, 1) = trackedPoints[i].y2; } CvMat pts1cv = pts1; CvMat pts2cv = pts2; matf mat(3, 3); CvMat matcv = mat; Mat_<uchar> status(1, trackedPoints.size()); CvMat statuscv = status; cvFindFundamentalMat(&pts1cv, &pts2cv, &matcv, method, param1, param2, &statuscv); #else Mat_<Vec2f> pts1(trackedPoints.size(), 1), pts2(trackedPoints.size(), 1); for (size_t i = 0; i < trackedPoints.size(); ++i) { pts1(i, 0) = Vec2f(trackedPoints[i].x1, trackedPoints[i].y1); pts2(i, 0) = Vec2f(trackedPoints[i].x2, trackedPoints[i].y2); } vector<unsigned char> status; Mat mat = findFundamentalMat(pts1, pts2, method, param1, param2, status); #endif if (inliers) { inliers->clear(); for (size_t i = 0; i < trackedPoints.size(); ++i) #ifdef OPENCV_2_1 if (status(0, i)) #else if (status[i]) #endif inliers->push_back(trackedPoints[i]); } //return H_out[1].inv().t() * (matf)mat * H_out[0].inv(); return mat; }
void VisualOdometry::confirmTrials() { // empty check if (trials.empty()) return; // triangulate landmarks // prepare points cv::Mat pts1(2, trials.size(), cv::DataType<float>::type), pts2(2, trials.size(), cv::DataType<float>::type); int col = 0; for (auto it = trials.begin(); it != trials.end(); it++) { cv::KeyPoint k1, k2; it->firstPointPair(k1, k2); pts1.at<float>(0, col) = k1.pt.x; pts1.at<float>(1, col) = k1.pt.y; pts2.at<float>(0, col) = k2.pt.x; pts2.at<float>(1, col) = k2.pt.y; col++; } cv::Mat pts3D; triangulate(pts1, pts2, pts3D); // convert to world coordinate int sframe = trials.back().getStartFrame(); cv::Mat posR, posT; graph.getPose(sframe, posR, posT); cv::Mat posRT = fullRT(posR, posT); pts3D = posRT * pts3D; int colcount = 0; for (auto it = trials.begin(); it != trials.end(); it++) { // add location to landmark data structure cv::Point3f p = cv::Point3f(pts3D.at<float>(0, colcount), pts3D.at<float>(1, colcount), pts3D.at<float>(2, colcount)); it->setLocation(p); colcount++; // add initial value of land mark to factor graph data structure graph.addLandMark(landmarkID, p); // move from trail to true landmarks landmarks[landmarkID] = *it; // add factors int curframe = it->getStartFrame(); for (int i = 0; i < it->getTraceSize(); i++) { cv::KeyPoint p1, p2; it->getPointPair(i, p1, p2); graph.addStereo(curframe, landmarkID, p1.pt, p2.pt); curframe++; } landmarkID++; } trials.clear(); }
void MimRec::calc_distance() { int countPoints = -1; for (int i = 0; i < numberOfFFP; i++) { cv::Vec2f pts1(getX(i), getY(i)); float ptsYAxis1 = getY(i); for (int j = i; j < numberOfFFP; j++) { countPoints++; cv::Vec2f pts2(getX(j), getY(j)); current_distances[countPoints] = cv::norm(pts1, pts2); float ptsYAxis2 = getY(j); distancesYAxis[countPoints] = ptsYAxis1 - ptsYAxis2; } } //special values for Lid Tightener cv::Vec2f middlePointrightEye((getX(47) + getX(44)) / 2, (getY(47) + getY(44)) / 2); cv::Vec2f ptlidright(getX(45), getY(45)); current_distances[0] = cv::norm(middlePointrightEye, ptlidright); cv::Vec2f middlePointLeftEye((getX(22) + getX(19)) / 2, (getY(22) + getY(19)) / 2); cv::Vec2f ptlidLeft(getX(20), getY(20)); current_distances[1] = cv::norm(middlePointLeftEye, ptlidLeft); cv::Vec2f ptLowerlidright(getX(46), getY(46)); current_distances[5461] = cv::norm(middlePointrightEye, ptLowerlidright); cv::Vec2f ptLowerlidLeft(getX(21), getY(21)); current_distances[5462] = cv::norm(middlePointLeftEye, ptLowerlidLeft); //angle between 7-8-30 for Lip Corner Depressor current_distances[5463] = acos((pow(current_distances[826], 2) + pow(current_distances[708], 2) - pow(current_distances[730], 2)) / (2 * abs(current_distances[826]) * abs(current_distances[708]))); current_distances[5463] *= 180.f / 3.14f; current_distances[5464] = acos((pow(current_distances[851], 2) + pow(current_distances[708], 2) - pow(current_distances[755], 2)) / (2 * abs(current_distances[708]) * abs(current_distances[851]))); current_distances[5464] *= 180.f / 3.14f; //angle between 6-5-30 for Lip Corner Puller current_distances[5465] = acos((pow(current_distances[535], 2) + pow(current_distances[511], 2) - pow(current_distances[633], 2)) / (2 * abs(current_distances[535]) * abs(current_distances[511]))); current_distances[5465] *= 180.f / 3.14f; current_distances[5466] = acos((pow(current_distances[560], 2) + pow(current_distances[511], 2) - pow(current_distances[658], 2)) / (2 * abs(current_distances[560]) * abs(current_distances[511]))); current_distances[5466] *= 180.f / 3.14f; //angle between 6 -55 -30 for funneler current_distances[5467] = acos((pow(current_distances[633], 2) + pow(current_distances[658], 2) - pow(current_distances[2710], 2)) / (2 * abs(current_distances[633]) * abs(current_distances[658]))); current_distances[5467] *= 180 / 3.14f; //angle between 22-19-16 for Inner Brow Raiser left current_distances[5468] = acos((pow(current_distances[1808], 2) + pow(current_distances[1547], 2) - pow(current_distances[1550], 2)) / (2 * abs(current_distances[1808]) * abs(current_distances[1547]))); current_distances[5468] *= 180.f / 3.14f; //angle between 41-44-47 für Inner Brow Raiser right current_distances[5469] = acos((pow(current_distances[3447], 2) + pow(current_distances[3633], 2) - pow(current_distances[3450], 2)) / (2 * abs(current_distances[3447]) * abs(current_distances[3633]))); current_distances[5469] *= 180.f / 3.14f; //angle between 22-17-19 for Outer Brow Raiser left current_distances[5470] = acos((pow(current_distances[1637], 2) + pow(current_distances[1808], 2) - pow(current_distances[1634], 2)) / (2 * abs(current_distances[1637]) * abs(current_distances[1808]))); current_distances[5470] *= 180.f / 3.14f; //angle between 44-42-47 für Outer Brow Raiser right current_distances[5471] = acos((pow(current_distances[3633], 2) + pow(current_distances[3512], 2) - pow(current_distances[3509], 2)) / (2 * abs(current_distances[3633]) * abs(current_distances[3512]))); current_distances[5471] *= 180.f / 3.14f; //cheek raiser cv::Vec2f cheek1(getX(26), getY(26)); current_distances[5472] = cv::norm(middlePointrightEye, cheek1); //lip stretcher current_distances[5473] = abs(getX(55) - getX(30)); }
long VisualOdometry::run(const cv::Mat& left_frame, const cv::Mat& right_frame) { // print frame info cout << "start>> landmarks:" << landmarks.size() << " trials:" << trials.size() << endl; // run time counting long t1 = cv::getTickCount(); // state machine switch (state) { case VO_START: graph.addFirstPose(frameCount); featureAssoc->initTrials(left_frame, right_frame, frameCount, trials); featureAssoc->visualizePair(trials); state = VO_BOOT; break; case VO_BOOT: featureAssoc->processImage(left_frame, right_frame, frameCount, landmarks, trials, unmatched); if (trials.size() < trialThre+200) { cout << "--confirm all initial trials--" << endl; confirmTrials(); trialState = false; state = VO_NORMAL; } graph.advancePoseTrivial(frameCount); break; case VO_NORMAL: // abnormal case if(landmarks.empty()) { cerr<<"!!!!!!!!!!!!ERROR::NO LANDMARKS!!!!!!!!!!!"<<endl; graph.advancePoseTrivial(frameCount); break; } // reset inlier for (auto it = landmarks.begin(); it != landmarks.end(); it++) { it->second.setInlier(false); } // run tracker featureAssoc->processImage(left_frame, right_frame, frameCount, landmarks, trials, unmatched); // print frame info cout << "track>> landmarks:" << landmarks.size() << " trials:" << trials.size() << endl; // confirm all trails if (trialState && trials.size() < trialThre) { cout << "--confirm all trials--" << endl; confirmTrials(); trialState = false; } // prepare points cv::Mat pts1(2, landmarks.size(), cv::DataType<float>::type), pts2(2, landmarks.size(), cv::DataType<float>::type); vector<cv::Point2f> imgPts; int colcount = 0; for (auto it = landmarks.begin(); it != landmarks.end(); it++) { cv::KeyPoint k1, k2; it->second.prevPointPair(k1, k2); pts1.at<float>(0, colcount) = k1.pt.x; pts1.at<float>(1, colcount) = k1.pt.y; pts2.at<float>(0, colcount) = k2.pt.x; pts2.at<float>(1, colcount) = k2.pt.y; imgPts.push_back(it->second.curLeftPoint().pt); colcount++; } // triangulate 3D points cv::Mat pts3D; triangulate(pts1, pts2, pts3D); // prepare 3D points cv::Mat R, Rvec, Tvec, inliers; vector<cv::Point3f> objPts; for (int c = 0; c < pts3D.cols; c++) { objPts.push_back(cv::Point3f(pts3D.at<float>(0, c), pts3D.at<float>(1, c), pts3D.at<float>(2, c))); } // incremental 3D-2D VO cv::solvePnPRansac(objPts, imgPts, camera.intrinsic, cv::noArray(), Rvec, Tvec, false, 3000, 1.0, 0.99, inliers, cv::SOLVEPNP_P3P); cv::Rodrigues(Rvec, R); //cout << R << Tvec << endl; //cout << inliers << endl; cout << "P3P points:" << imgPts.size() << " inliers:" << inliers.rows << endl; //set inliers int j = 0, count = 0; if (inliers.rows > 0) { for (auto it = landmarks.begin(); it != landmarks.end(); it++) { if (count++ == inliers.at<int>(j, 0)) { it->second.setInlier(true); if (++j >= inliers.rows) break; } } } else { cerr<<"!!!!!!!!!!!!WARNING::PNP FAILED!!!!!!!!!!!"<<endl; R = cv::Mat::eye(3, 3, cv::DataType<float>::type); Tvec = cv::Mat::zeros(3, 1, cv::DataType<float>::type); cv::randn(Tvec,cv::Scalar::all(0),cv::Scalar::all(0.001)); } // add stereo factors for (auto it = landmarks.begin(); it != landmarks.end(); it++) { if (it->second.isInlier()) { cv::KeyPoint p1, p2; it->second.curPointPair(p1, p2); graph.addStereo(frameCount, it->first, p1.pt, p2.pt); } } // add initial pose estimation allR.push_back(R); allT.push_back(Tvec); graph.advancePose(frameCount, R, Tvec); //--------loop constraint hack------------- #if LOOP if(frameCount == 3700) graph.addLoopConstraint(0,3700); #endif //--------loop constraint hack-------------- // run bundle adjustment long u1 = cv::getTickCount(); //graph.batchUpdate(); graph.increUpdate(); long u2 = cv::getTickCount(); cout << "optimization:" << float(u2 - u1) / cv::getTickFrequency() << endl; updateLandmark(); if (!trialState && landmarks.size() < landmarkThre) { cout << "--init new trails--" << endl; featureAssoc->refreshTrials(left_frame, right_frame, frameCount, unmatched, trials); featureAssoc->visualizePair(trials); trialState = true; if (trials.size() < directAddThre || landmarks.size() < lowlandThre) { //just add them don't wait cerr<<"!!!!!!!!!!!!WARNING::LOW LANDMARKS!!!!!!!!!!!"<<endl; confirmTrials(); trialState = false; } } break; } // advance frame count number frameCount++; // count runing time long t2 = cv::getTickCount(); // print info cout << "end>> landmarks:" << landmarks.size() << " trials:" << trials.size() << endl; cout << "time:" << float(t2 - t1) / cv::getTickFrequency() << endl; // return running time return t2 - t1; }