lhark/papillon
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

Fix recursive stabilisation

Browse source
This commit is contained in:
lhark 2016-06-07 12:56:04 +02:00
parent 90f55e89cd
commit 40f52fbf03
1 changed files with 38 additions and 24 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

62
src/papillon.cpp
View file

@ -17,7 +17,7 @@ class Traite_image {
const static int THRESHOLD_DETECT_SENSITIVITY = 10;
const static int BLUR_SIZE = 5;
const static int THRESHOLD_MOV = 5;
constexpr static float MOVEMENT_THRES = 0.1;
constexpr static float MOVEMENT_THRES = 0.05;
constexpr static float FLOW_MIN_QUAL = 0.01;
const static int FLOW_MIN_DIST = 20;
@ -80,10 +80,12 @@ class Traite_image {
}
Mat next_stab;
stabiliseImg(prev, next, next_stab);
trackFeatures(prev, next);
stabiliseImg(next, next_stab);
trackingOptFlow(prev, next_stab, next_stab);
Mat next_stab2;
stabiliseImg(prev, next, next_stab2);
trackFeatures(prev, next);
stabiliseImg(next, next_stab2);
trackingOptFlow(prev, next_stab2, output);
//searchForMovementOptFlow(prev_cropped, next_stab_cropped, output);
@ -98,23 +100,14 @@ class Traite_image {
prev = next.clone();
}
//int to string helper function
string intToString(int number){
//this function has a number input and string output
std::stringstream ss;
ss << number;
return ss.str();
}
void stabiliseImg(Mat prev, Mat cur, Mat &output){
void trackFeatures(Mat prev, Mat cur) {
Mat cur_grey, prev_grey;
cvtColor(cur, cur_grey, COLOR_BGR2GRAY);
cvtColor(prev, prev_grey, COLOR_BGR2GRAY);
// vector from prev to cur
vector <Point2f> prev_corner, cur_corner;
vector <Point2f> prev_corner2, cur_corner2;
vector <uchar> status;
vector <float> err;
@ -122,15 +115,18 @@ class Traite_image {
calcOpticalFlowPyrLK(prev_grey, cur_grey, prev_corner, cur_corner, status, err);
// weed out bad matches
prev_ftr.clear();
cur_ftr.clear();
prev_ftr.resize(0);
cur_ftr.resize(0);
for(size_t i=0; i < status.size(); i++) {
if(status[i]) {
prev_ftr.push_back(prev_corner[i]);
cur_ftr.push_back(cur_corner[i]);
}
}
}
void stabiliseImg(Mat cur, Mat &output){
T = estimateRigidTransform(prev_ftr, cur_ftr, true); // false = rigid transform, no scaling/shearing
if(T.data == NULL)
@ -145,9 +141,11 @@ class Traite_image {
cur2.copyTo(output);
}
void warpPoints(vector<Point2f> p, vector<Point2f> &p_warp, Mat T, bool invert=false) {
Mat H;
if(invert)
invertAffineTransform(T, T);
invertAffineTransform(T, H);
p_warp.clear();
for(size_t i=0; i < p.size(); ++i) {
@ -157,29 +155,34 @@ class Traite_image {
src.at<double>(1,0)=p[i].y;
src.at<double>(2,0)=1.0;
Mat dst = T*src; //USE MATRIX ALGEBRA
Mat dst = H*src; //USE MATRIX ALGEBRA
p_warp.push_back(Point2f(dst.at<double>(0,0),dst.at<double>(1,0)));
}
}
void trackingOptFlow(Mat prev, Mat cur, Mat &output) {
cur.copyTo(output);
vector <Point2f> cur_ftr_stab;
Mat T = estimateRigidTransform(prev_ftr, cur_ftr, true); // false = rigid transform, no scaling/shearing
if(T.data == NULL)
last_T.copyTo(T);
else
T.copyTo(last_T);
//T = estimateRigidTransform(prev_ftr, cur_ftr, true); // false = rigid transform, no scaling/shearing
//if(T.data == NULL)
// last_T.copyTo(T);
//else
// T.copyTo(last_T);
warpPoints(cur_ftr, cur_ftr_stab, T, true);
vector <Point2f> objects;
vector <float> flow_norm;
for(size_t i=0; i < prev_ftr.size(); ++i) {
float flow_norm = norm(prev_ftr[i] - cur_ftr_stab[i]) / prev.size().height;
flow_norm.push_back(norm(prev_ftr[i] - cur_ftr_stab[i]) / prev.size().height);
line(output, prev_ftr[i], cur_ftr[i], Scalar(200,0,0),1);
line(output, prev_ftr[i], cur_ftr_stab[i], Scalar(0,200,0),1);
if(flow_norm > MOVEMENT_THRES) {
}
for(size_t i=0; i < flow_norm.size(); ++i) {
if(flow_norm[i] > MOVEMENT_THRES) {
objects.push_back(cur_ftr_stab[i]);
prev_ftr.erase(prev_ftr.begin() + i);
cur_ftr.erase(cur_ftr.begin() + i);
@ -191,6 +194,7 @@ class Traite_image {
}
}
inline bool isFlowCorrect(Point2f u)
{
return !cvIsNaN(u.x) && !cvIsNaN(u.y) && fabs(u.x) < 1e9 && fabs(u.y) < 1e9;
@ -215,6 +219,16 @@ class Traite_image {
pub_cmd.publish(twist);
}
//int to string helper function
string intToString(int number){
//this function has a number input and string output
std::stringstream ss;
ss << number;
return ss.str();
}
};