diff options
| -rw-r--r-- | src/camera.cpp | 1 | ||||
| -rw-r--r-- | src/feature.cpp | 16 | ||||
| -rw-r--r-- | src/feature.h | 11 | ||||
| -rw-r--r-- | src/main.cpp | 45 | ||||
| -rw-r--r-- | src/state.cpp | 98 | ||||
| -rw-r--r-- | src/state.h | 50 | ||||
| -rw-r--r-- | src/types.h | 5 | ||||
| -rw-r--r-- | src/vision.cpp | 135 | ||||
| -rw-r--r-- | src/vision.h | 11 |
9 files changed, 260 insertions, 112 deletions
diff --git a/src/camera.cpp b/src/camera.cpp index 7e1c1b3..415bf35 100644 --- a/src/camera.cpp +++ b/src/camera.cpp @@ -182,6 +182,7 @@ Camera::body2img ( const Vector3d &xb ) const Vector3d xc,xi; xc = Rc2b().transpose()*xb; xi = K()*xc; + xi /= xi[2]; return xi; } /* ----- end of method Camera::body2img ----- */ diff --git a/src/feature.cpp b/src/feature.cpp index 22bf776..66b0677 100644 --- a/src/feature.cpp +++ b/src/feature.cpp @@ -26,7 +26,7 @@ *-------------------------------------------------------------------------------------- */ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr, - const Vector3d &xbw, const Quaterniond &q, double z) + const Vector3d &xbw, const Quaterniond &q, double z, Mat &p) { _id = id; Vector3d xib; @@ -35,6 +35,7 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr, X0 = X; xb0w = xbw; q0 = q; + _patch = p; } /* ----- end of method Feature::Feature (constructor) ----- */ /* @@ -45,7 +46,7 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xr, *-------------------------------------------------------------------------------------- */ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xbw, - const Quaterniond &q) + const Quaterniond &q, Mat &p) { _id = id; Vector3d xib, pib; @@ -59,6 +60,7 @@ Feature::Feature ( int id, const Vector3d &xs, const Vector3d &xbw, X0 = X; xb0w = xbw; q0 = q; + _patch = p; } @@ -953,11 +955,15 @@ Feature::P0 ( measurement_type t ) double p0 = FEATURECOVX; double p1 = FEATURECOVY; double p2; + if (t==REFLECTION) { #ifdef INITDEPTH p2 = FEATURECOVRHO; #else p2 = FEATURECOVRHO_MONO; #endif + } else { + p2 = FEATURECOVRHO_MONO; + } P << p0, 0., 0., 0., p1, 0., 0., 0., p2; @@ -1107,3 +1113,9 @@ Feature::asVector ( const Vector3d &m ) return ; } /* ----- end of method Feature::asVector ----- */ +Mat +Feature::patch ( ) +{ + return _patch ; +} /* ----- end of method Feature::patch ----- */ + diff --git a/src/feature.h b/src/feature.h index d5e84b4..1c0718d 100644 --- a/src/feature.h +++ b/src/feature.h @@ -1,5 +1,6 @@ #ifndef feature_INC #define feature_INC +#include <cv.h> #include <Eigen/Dense> #include <iostream> #include "types.h" @@ -13,11 +14,11 @@ #define FEATURECOVRHO 25e-4 /* */ #define FEATURECOVRHO_MONO 0.5 /* */ #define RHO_0 1./10. /* */ -#define INLIER_THRESHOLD 5.9915 /* */ #define RANSAC_LI_THRESHOLD 4e-5 /* */ #define RANSAC_HI_THRESHOLD 5e-2 /* */ #define INITDEPTH +using cv::Mat; using Eigen::Dynamic; using Eigen::Matrix; using Eigen::MatrixXd; @@ -40,15 +41,16 @@ class Feature public: /* ==================== LIFECYCLE ======================================= */ Feature ( int id, const Vector3d &xs, const Vector3d &xr, - const Vector3d &xbw, const Quaterniond &q, double z); + const Vector3d &xbw, const Quaterniond &q, double z, Mat &p); Feature ( int id, const Vector3d &xs, const Vector3d &xbw, - const Quaterniond &q); + const Quaterniond &q, Mat &p); /* ==================== ACCESSORS ======================================= */ int id(); Matrix<double,3,3> P0(measurement_type t); Vector3d asVector(); UTM utm(Vector3d &xbw, Quaterniond &q); + Mat patch(); /* ==================== MUTATORS ======================================= */ void asVector(const Vector3d &m); @@ -91,9 +93,10 @@ class Feature /* ==================== DATA MEMBERS ======================================= */ int _id; + Mat _patch; + Quaterniond q0; Vector3d X; Vector3d X0; - Quaterniond q0; Vector3d xb0w; }; /* ----- end of class Feature ----- */ diff --git a/src/main.cpp b/src/main.cpp index e7e08a5..bd31da9 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -170,51 +170,10 @@ imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q) Vision viz; viz.open(msg.image_names[0],cam); - strcat(msg.image_names[0],".txt"); - FILE *fin; - if ((fin=fopen(msg.image_names[0], "r"))==NULL) { - err_sys("fopen: %s", msg.image_names[0]); - } - int id,sx,sy,rx,ry; - int Nmatched; - while ((Nmatched=fscanf(fin,"%d,%d,%d,%d,%d", &id, &sx, &sy, &rx, &ry))!=EOF) { - measurement_t m; - m.id = id; - // Points in the image frame - Vector3d xi, xir; - xi << sx, sy, 1.; - m.source = cam.img2body(xi); - if (Nmatched==5) { - xir << rx, ry, 1.; - m.reflection = cam.img2body(xir); - m.z_type = REFLECTION; - } else { - m.z_type = MONO; - } - // Points in the camera frame - z.push_back(m); - } - if (fclose(fin)==EOF) { - err_sys("fclose"); - } - // The measured values - //viz.drawMeasurements(z,cam, cv::Scalar(0,255,0), cv::Scalar(0,180,0),false); - // The predicted values - std::vector<measurement_t> ykk1; - std::vector<measurement_t> newFeatures; - viz.acquireFeatures(cam, newFeatures); - viz.drawMeasurements(newFeatures, cam, cv::Scalar(0,0,255), cv::Scalar(0,0,180),false); -#if STATESIZE==13 - //mu.featuresAsMeasurements(ykk1); -#else - //mu.featuresAsMeasurements(ykk1,q); -#endif - //viz.drawMeasurements(ykk1, cam, cv::Scalar(255,0,0), cv::Scalar(180,0,0),true); - viz.show(); #if STATESIZE==13 - //mu.handle_measurements(z); + mu.doMeasurements(z,viz,cam); #else - mu.handle_measurements(z,q); + mu.doMeasurements(z,viz,q,cam); #endif } diff --git a/src/state.cpp b/src/state.cpp index 8a31535..d43be62 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -50,9 +50,9 @@ State::pos ( const UTM &utm ) */ MatrixXd #if STATESIZE==13 -State::H ( const std::vector<measurement_t> &z ) +State::H ( const vector<measurement_t> &z ) #else -State::H ( const Quaterniond &q, const std::vector<measurement_t> &z ) +State::H ( const Quaterniond &q, const vector<measurement_t> &z ) #endif { #if STATESIZE==13 @@ -114,7 +114,7 @@ State::H ( const Quaterniond &q, const std::vector<measurement_t> &z ) *-------------------------------------------------------------------------------------- */ int -State::Hrows ( const std::vector<measurement_t> &z) +State::Hrows ( const vector<measurement_t> &z) { int rows = 0; for (auto i=z.begin(); i!=z.end(); ++i) { @@ -146,7 +146,7 @@ State::Hrows ( const std::vector<measurement_t> &z) *-------------------------------------------------------------------------------------- */ MatrixXd -State::R ( const std::vector<measurement_t> &z ) +State::R ( const vector<measurement_t> &z ) { int rows = Hrows(z); MatrixXd r; @@ -183,10 +183,10 @@ State::R ( const std::vector<measurement_t> &z ) MatrixXd #if STATESIZE==13 State::partialUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z) + const vector<measurement_t> &z) #else State::partialUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z, const Quaterniond &q ) + const vector<measurement_t> &z, const Quaterniond &q ) #endif { MatrixXd K; @@ -229,10 +229,10 @@ State::partialUpdate( const MatrixXd &h, const MatrixXd &S, void #if STATESIZE==13 State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z) + const vector<measurement_t> &z) #else State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z, const Quaterniond &q) + const vector<measurement_t> &z, const Quaterniond &q) #endif { MatrixXd K; @@ -259,9 +259,9 @@ State::kalmanUpdate( const MatrixXd &h, const MatrixXd &S, */ Matrix<double,Dynamic,1> #if STATESIZE==13 -State::innovation( const std::vector<measurement_t> &z) +State::innovation( const vector<measurement_t> &z) #else -State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q) +State::innovation( const vector<measurement_t> &z, const Quaterniond &q) #endif { #if STATESIZE==13 @@ -315,13 +315,13 @@ State::innovation( const std::vector<measurement_t> &z, const Quaterniond &q) */ void #if STATESIZE==13 -State::handle_measurements ( const std::vector<measurement_t> &z) +State::handle_measurements ( const vector<measurement_t> &z) #else -State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterniond &q) +State::handle_measurements ( const vector<measurement_t> &z, const Quaterniond &q) #endif { - std::vector<measurement_t> featuresToAdd; - std::vector<measurement_t> featuresToUpdate; + vector<measurement_t> featuresToAdd; + vector<measurement_t> featuresToUpdate; double zmeas = body->ned()[2]; for (auto i=z.begin(); i!=z.end(); ++i) { if (i->z_type==HEIGHT) { @@ -397,9 +397,9 @@ State::handle_measurements ( const std::vector<measurement_t> &z, const Quaterni MatrixXd #if STATESIZE==13 -State::blockSI ( const std::vector<measurement_t> &z) +State::blockSI ( const vector<measurement_t> &z) #else -State::blockSI ( const std::vector<measurement_t> &z, const Quaterniond &q ) +State::blockSI ( const vector<measurement_t> &z, const Quaterniond &q ) #endif { #if STATESIZE==13 @@ -466,9 +466,9 @@ State::Pyy ( int id ) void #if STATESIZE==13 -State::addFeatures ( std::vector<measurement_t> &F, double z) +State::addFeatures ( vector<measurement_t> &F, double z) #else -State::addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z) +State::addFeatures(vector<measurement_t> &F, const Quaterniond &q, double z) #endif { #if STATESIZE==13 @@ -483,12 +483,15 @@ State::addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z Feature *f; if (i->z_type==REFLECTION) { #ifdef INITDEPTH - f = new Feature(i->id, i->source, i->reflection, pos, q, z); + f = new Feature(i->id, i->source, i->reflection, pos, q, z, i->patch); #else /* ----- not INITDEPTH ----- */ - f = new Feature(i->id, i->source, pos, q); + f = new Feature(i->id, i->source, pos, q, i->patch); #endif /* ----- not INITDEPTH ----- */ + } else if (i->z_type==MONO) { + f = new Feature(i->id, i->source, pos, q, i->patch); } else { - f = new Feature(i->id, i->source, pos, q); + fprintf(stderr, "Measurement type: %d not known, quitting\n", i->z_type); + exit(1); } if (!f->sane()) { delete f; @@ -868,9 +871,9 @@ State::shrinkP ( int i ) void #if STATESIZE==13 -State::ransacUpdate ( std::vector<measurement_t> &z ) +State::ransacUpdate ( vector<measurement_t> &z ) #else -State::ransacUpdate ( std::vector<measurement_t> &z, const Quaterniond &q ) +State::ransacUpdate ( vector<measurement_t> &z, const Quaterniond &q ) #endif { #if STATESIZE==13 @@ -883,10 +886,10 @@ State::ransacUpdate ( std::vector<measurement_t> &z, const Quaterniond &q ) int N0 = z.size(); int N = z.size(); - std::vector<measurement_t> inliers; + vector<measurement_t> inliers; for (int i=0; i<N && i<N0; ++i) { measurement_t zi = z[i]; - std::vector<measurement_t> Z; + vector<measurement_t> Z; Z.push_back(zi); MatrixXd h; #if STATESIZE==13 @@ -941,7 +944,7 @@ State::ransacUpdate ( std::vector<measurement_t> &z, const Quaterniond &q ) #endif // High innovation update - std::vector<measurement_t> hi_inliers; + vector<measurement_t> hi_inliers; for (auto i=z.begin(); i!=z.end(); ++i) { bool found = false; for (auto j=inliers.begin(); j!=inliers.end(); ++j) { @@ -1046,9 +1049,9 @@ State::quaternion_covariance ( const Matrix<double,4,4> &covq ) */ void #if STATESIZE==13 -State::featuresAsMeasurements ( std::vector<measurement_t> &yk ) +State::featuresAsMeasurements ( vector<measurement_t> &yk ) #else -State::featuresAsMeasurements ( std::vector<measurement_t> &yk, +State::featuresAsMeasurements ( vector<measurement_t> &yk, const Quaterniond &q) #endif { @@ -1065,10 +1068,11 @@ State::featuresAsMeasurements ( std::vector<measurement_t> &yk, pbr << h(4), h(5), 1; measurement_t z; - z.id = 0; + z.id = (*i)->id(); z.source = (*i)->p2x(pbs); z.reflection = (*i)->p2x(pbr); z.z_type = REFLECTION; + z.patch = (*i)->patch(); // Get ellipse Matrix<double,6,6> s; @@ -1081,3 +1085,39 @@ State::featuresAsMeasurements ( std::vector<measurement_t> &yk, return ; } /* ----- end of method State::featuresAsMeasurements ----- */ +void +#if STATESIZE==13 +State::doMeasurements ( vector<measurement_t> &z, Vision &viz, Camera &cam ) +#else +State::doMeasurements ( vector<measurement_t> &z, Vision &viz, + const Quaterniond &q, Camera &cam) +#endif +{ + measurement_t hgt = z[0]; + assert (hgt.z_type==HEIGHT); + double hgtmeas = hgt.height; + vector<measurement_t> newFeatures; + if (features.size()<MINFEATURES) { + viz.acquireFeatures(cam, newFeatures); + } + // Measure existing features + vector<measurement_t> currentFeatures, featureZ; +#if STATESIZE==13 + featuresAsMeasurements(currentFeatures); +#else + featuresAsMeasurements(currentFeatures,q); +#endif + viz.measurements(cam, currentFeatures, featureZ); + + // Initialize new features + if (features.size()<MINFEATURES) { +#if STATESIZE==13 + addFeatures(newFeatures, hgtmeas); +#else + addFeatures(newFeatures, q, hgtmeas); +#endif + } + + return ; +} /* ----- end of method State::doMeasurements ----- */ + diff --git a/src/state.h b/src/state.h index 55e2d86..8098415 100644 --- a/src/state.h +++ b/src/state.h @@ -9,8 +9,10 @@ #include "camera.h" #include "feature.h" #include "types.h" +#include "vision.h" -#define MAXFEATURES 30 +#define MAXFEATURES 40 +#define MINFEATURES 20 #define COVBIAS 2e-5 //#define FASTMOTIONMODEL // Uncomment this to perform motion model update on all features at once #define DORANSAC /* */ @@ -24,6 +26,7 @@ using Eigen::Vector3d; using std::cout; using std::cerr; using std::endl; +using std::vector; /* * ===================================================================================== @@ -45,20 +48,20 @@ class State Feature *featureById(int id); #if STATESIZE==13 MatrixXd F(const Vector3d &w, double dt); - MatrixXd H ( const std::vector<measurement_t> &z ); - MatrixXd blockSI( const std::vector<measurement_t> &z ); + MatrixXd H ( const vector<measurement_t> &z ); + MatrixXd blockSI( const vector<measurement_t> &z ); Quaterniond qhat(); - void featuresAsMeasurements(std::vector<measurement_t> &yk); + void featuresAsMeasurements(vector<measurement_t> &yk); #else MatrixXd F(const Quaterniond &q, const Vector3d &w, double dt); - MatrixXd H ( const Quaterniond &q, const std::vector<measurement_t> &z ); - MatrixXd blockSI( const std::vector<measurement_t> &z, const Quaterniond &q); - void featuresAsMeasurements(std::vector<measurement_t> &yk, + MatrixXd H ( const Quaterniond &q, const vector<measurement_t> &z ); + MatrixXd blockSI( const vector<measurement_t> &z, const Quaterniond &q); + void featuresAsMeasurements(vector<measurement_t> &yk, const Quaterniond &q); #endif - int Hrows( const std::vector<measurement_t> &z ); + int Hrows( const vector<measurement_t> &z ); MatrixXd Q(double dt); - MatrixXd R( const std::vector<measurement_t> &z ); + MatrixXd R( const vector<measurement_t> &z ); Matrix<double,STATESIZE,STATESIZE> Pxx(); Matrix<double,STATESIZE,3> Pxy(int id); Matrix<double,3,3> Pyy(int id); @@ -69,26 +72,29 @@ class State void asVector(const Matrix<double,Dynamic,1> &m); void pos(const UTM &utm); #if STATESIZE==13 - void handle_measurements( const std::vector<measurement_t> & z); + void doMeasurements( vector<measurement_t> &z, Vision &viz, Camera &cam); + void handle_measurements( const vector<measurement_t> & z); void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z); + const vector<measurement_t> &z); void motionModel(const Vector3d &acc, const Vector3d &ang, const double dt); MatrixXd partialUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z); + const vector<measurement_t> &z); void Pkk1 ( const Vector3d &ang, const double dt); - void ransacUpdate(std::vector<measurement_t> &z); + void ransacUpdate(vector<measurement_t> &z); void quaternion_covariance(const Matrix<double,4,4> &covq); void qhat(const Quaterniond &q); #else - void handle_measurements( const std::vector<measurement_t> & z, const Quaterniond &q); + void doMeasurements( vector<measurement_t> &z, Vision &viz, + const Quaterniond &q, Camera &cam); + void handle_measurements( const vector<measurement_t> & z, const Quaterniond &q); void kalmanUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z, const Quaterniond &q); + const vector<measurement_t> &z, const Quaterniond &q); void motionModel(const Vector3d &acc, const Vector3d &ang, const Quaterniond &q, const double dt); MatrixXd partialUpdate( const MatrixXd &h, const MatrixXd &S, - const std::vector<measurement_t> &z, const Quaterniond &q ); + const vector<measurement_t> &z, const Quaterniond &q ); void Pkk1 ( const Vector3d &ang, const Quaterniond &q, const double dt); - void ransacUpdate(std::vector<measurement_t> &z, const Quaterniond &q); + void ransacUpdate(vector<measurement_t> &z, const Quaterniond &q); #endif void initializePi(int i, const Matrix<double,3,3> &Pi); void position_covariance(const Matrix<double,3,3> &p); @@ -99,9 +105,9 @@ class State /* ==================== OPERATORS ======================================= */ #if STATESIZE==13 - Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z); + Matrix<double,Dynamic,1> innovation( const vector<measurement_t> &z); #else - Matrix<double,Dynamic,1> innovation( const std::vector<measurement_t> &z, const Quaterniond &q); + Matrix<double,Dynamic,1> innovation( const vector<measurement_t> &z, const Quaterniond &q); #endif void unicsv(); @@ -113,9 +119,9 @@ class State private: /* ==================== METHODS ======================================= */ #if STATESIZE==13 - void addFeatures(std::vector<measurement_t> &F, double z); + void addFeatures(vector<measurement_t> &F, double z); #else - void addFeatures(std::vector<measurement_t> &F, const Quaterniond &q, double z); + void addFeatures(vector<measurement_t> &F, const Quaterniond &q, double z); #endif void expandP ( const Matrix3d &Pi); void shrinkP( int i ); @@ -127,7 +133,7 @@ class State char utm_c; int utm_i; std::list<Feature *> features; - std::vector<int> blacklist; + vector<int> blacklist; }; /* ----- end of class State ----- */ diff --git a/src/types.h b/src/types.h index 8b0e8a2..d3aeb01 100644 --- a/src/types.h +++ b/src/types.h @@ -1,10 +1,12 @@ #ifndef types_INC #define types_INC +#include <cv.h> #include <Eigen/Dense> #define MAXLINE 8192 #define MAXFILENAME 1024 -#define STATESIZE 13 /* Set to 13 for quaternion estimation, or 9 for quaternion as input */ +#define STATESIZE 9 /* Set to 13 for quaternion estimation, or 9 for quaternion as input */ +#define INLIER_THRESHOLD 12. using Eigen::Matrix; using Eigen::Matrix3d; using Eigen::Quaterniond; @@ -20,6 +22,7 @@ typedef struct { Vector3d source, reflection; double height; Vector2d Ssrc, Sref; + cv::Mat patch; } measurement_t; // A struct for storing PVA covariance. diff --git a/src/vision.cpp b/src/vision.cpp index 7d1931d..0227053 100644 --- a/src/vision.cpp +++ b/src/vision.cpp @@ -41,14 +41,20 @@ Vision::Vision () void Vision::open ( const char *fn, const Camera &cam ) { - Mat bayered ; + Mat bayered, unbayered ; bayered = cv::imread(fn, -1); if (!bayered.data) { fprintf(stderr, "Could not read %s.\n", fn); exit(1); } - cv::cvtColor(bayered, original, CV_BayerBG2BGR ,3); - display = original.clone(); + cv::cvtColor(bayered, unbayered, CV_BayerBG2BGR ,3); + unbayered.convertTo(original, CV_32FC3); + Mat luv; + cv::cvtColor(original, gray, CV_BGR2GRAY); + //vector<Mat> luv_channels; + //cv::split(luv,luv_channels); + //gray = luv_channels[0]; + display = unbayered.clone(); return ; } /* ----- end of method Vision::open ----- */ @@ -66,7 +72,6 @@ Vision::drawMeasurements ( const vector<measurement_t> &z, const Camera &cam, cv::Scalar ellcol(0,0,200); for (auto i=z.begin(); i!=z.end(); ++i) { if (i->z_type==HEIGHT) continue; - cerr << i->id << endl; Vector3d xis, xir; xis = cam.body2img(i->source); xis /= xis[2]; @@ -86,8 +91,8 @@ Vision::drawMeasurements ( const vector<measurement_t> &z, const Camera &cam, yr = 2*sqrt(i->Sref(1)); // Create rotated rect for ellipse - cv::RotatedRect rrs(ps, cv::Size(2*5.9*xs, 2*5.9*ys), 0.); - cv::RotatedRect rrr(pr, cv::Size(2*5.9*xr, 2*5.9*yr), 0.); + cv::RotatedRect rrs(ps, cv::Size(2*INLIER_THRESHOLD*xs, 2*INLIER_THRESHOLD*ys), 0.); + cv::RotatedRect rrr(pr, cv::Size(2*INLIER_THRESHOLD*xr, 2*INLIER_THRESHOLD*yr), 0.); cv::ellipse(display, rrs, ellcol ); cv::ellipse(display, rrr, ellcol); } @@ -121,9 +126,12 @@ void Vision::acquireFeatures ( const Camera &cam, vector<measurement_t> &z ) { vector<cv::Point2f> corners; - Mat gray; - cv::cvtColor(original, gray, CV_BGR2GRAY); - cv::goodFeaturesToTrack(gray, corners, 50, 0.1, 20); + + Mat mask = Mat::zeros(gray.size(),CV_8UC1); + cv::Rect roi(50,50,gray.cols-100, gray.rows-100); + mask(roi) = 255*Mat::ones(roi.size(),CV_8UC1); + + cv::goodFeaturesToTrack(gray, corners, 50, 0.1, 20, mask); for (auto i=corners.begin(); i!=corners.end(); ++i) { measurement_t m; m.id = _id++; @@ -132,9 +140,118 @@ Vision::acquireFeatures ( const Camera &cam, vector<measurement_t> &z ) xi << i->x, i->y, 1.; m.source = cam.img2body(xi); m.reflection << 0,0,1; + getTemplate(m.patch,xi); + if (m.patch.cols==0 || m.patch.rows==0) continue; z.push_back(m); } return ; } /* ----- end of method Vision::acquireFeatures ----- */ +void +Vision::getTemplate( Mat &p, const Vector3d &pt) +{ + cv::Rect imrect(0,0,gray.cols,gray.rows); + assert (PATCHSIZE%2==1); // patch size must be odd + cv::Point pti; + pti.x = (int) pt(0); + pti.y = (int)pt(1); + cv::Point topLeft = pti - cv::Point(PATCHSIZE/2,PATCHSIZE/2); + cv::Size sz(PATCHSIZE,PATCHSIZE); + cv::Rect roi(topLeft, sz); + roi &= imrect; + p = gray(roi).clone(); +} + +void +Vision::measurements ( const Camera &cam, const vector<measurement_t> &zin, + vector<measurement_t> &zout) +{ + for (auto i=zin.begin(); i!=zin.end(); ++i) { + measurement_t z; + measure(cam,*i, z); + zout.push_back(z); + } + return ; +} /* ----- end of method Vision::measurements ----- */ + +/* + *-------------------------------------------------------------------------------------- + * Class: Vision + * Method: Vision :: measure + * Description: Performs a measurement on zin and stores it in zout. + *-------------------------------------------------------------------------------------- + */ +void +Vision::measure ( const Camera &cam, const measurement_t &zin, measurement_t &zout ) +{ + zout.id = zin.id; + + // Create ROIs + RotatedRect searchrr; // Rotated rect describing search ellipse + Rect searchr; // Rect describing search ellipse + searchRegion(cam, zin, searchrr); + searchr = searchrr.boundingRect(); + + // Add margin around search region + cv::Point offset(PATCHSIZE/2,PATCHSIZE/2); + cv::Size enlarge(2*(PATCHSIZE/2),2*(PATCHSIZE/2)); + Rect roi = (searchr-offset) + enlarge; + Mat sr = gray(roi); + + // Match template + Mat result; + result.create( sr.cols-zin.patch.cols+1, sr.rows-zin.patch.rows+1, CV_32FC1 ); + matchTemplate( sr, zin.patch, result, CV_TM_CCORR_NORMED); + + // Mask the ellipse around the result. + Mat mask = Mat::zeros(result.size(), CV_8UC1); + RotatedRect ellipserr; + cv::Size sz = searchr.size(); + ellipserr.size = sz; + ellipserr.center = cv::Point2f(0.5*sz.width,0.5*sz.height); + ellipserr.angle = 0; + cv::ellipse(mask, ellipserr, cv::Scalar(255), -1 ); + cerr << searchrr.size << endl; + cerr << searchr.size() << endl; + double minval, maxval; + int minidx[2], maxidx[2]; + cv::minMaxIdx(result, &minval, &maxval, minidx, maxidx, mask); + + // Display result + cv::Point maxpt(maxidx[0], maxidx[1]); + Mat sr8, patch8; + sr.convertTo(sr8, CV_8UC1); + zin.patch.convertTo(patch8, CV_8UC1); + cv::circle(sr8,maxpt+offset,4,cv::Scalar(255),-1); + cv::Point spt(PATCHSIZE/2, PATCHSIZE/2); + cv::circle(patch8,spt,2,cv::Scalar(255),-1); + cerr << maxval << endl; + cv::imshow("s. region", sr8); + cv::imshow("patch", patch8); + cv::imshow("result", result); + cv::imshow("mask", mask); + cv::waitKey(0); + return ; +} /* ----- end of method Vision::measure ----- */ + +void +Vision::searchRegion ( const Camera &cam, const measurement_t &z, cv::RotatedRect &rr ) +{ + Vector3d xis; + xis = cam.body2img(z.source); + //xis /= xis[2]; + cv::Point ps; + ps.x = xis[0]; + ps.y = xis[1]; + + double xs, ys; + xs = 2*sqrt(z.Ssrc(0)); + ys = 2*sqrt(z.Ssrc(1)); + + // Create rotated rect for ellipse + rr = cv::RotatedRect(ps, cv::Size(2*INLIER_THRESHOLD*xs, 2*INLIER_THRESHOLD*ys), 0.); + + return ; +} /* ----- end of method Vision::searchRegion ----- */ + int Vision::_id = 1; diff --git a/src/vision.h b/src/vision.h index 6f324c1..a8ed318 100644 --- a/src/vision.h +++ b/src/vision.h @@ -8,8 +8,10 @@ #include "camera.h" #include "types.h" - +#define PATCHSIZE 51 /* must be odd */ using cv::Mat; +using cv::RotatedRect; +using cv::Rect; using Eigen::Vector2d; using Eigen::Vector3d; using Eigen::Matrix; @@ -39,6 +41,11 @@ class Vision /* ==================== OPERATORS ======================================= */ void open(const char *fn, const Camera &cam); void acquireFeatures(const Camera &cam, vector<measurement_t> &z); + void getTemplate( Mat &p, const Vector3d &pt); + void measure( const Camera &cam, const measurement_t &zin, measurement_t &zout); + void measurements( const Camera &cam, const vector<measurement_t> &zin, + vector<measurement_t> &zout); + void searchRegion( const Camera &cam, const measurement_t &z, cv::RotatedRect &rr); protected: /* ==================== METHODS ======================================= */ @@ -49,7 +56,7 @@ class Vision /* ==================== METHODS ======================================= */ /* ==================== DATA MEMBERS ======================================= */ - Mat original, display; + Mat original, gray, display; static int _id; }; /* ----- end of class Vision ----- */ |