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