diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 48 |
1 files changed, 40 insertions, 8 deletions
diff --git a/src/state.cpp b/src/state.cpp index d43be62..5a334c5 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -1049,9 +1049,9 @@ State::quaternion_covariance ( const Matrix<double,4,4> &covq ) */ void #if STATESIZE==13 -State::featuresAsMeasurements ( vector<measurement_t> &yk ) +State::featuresAsMeasurements ( vector<measurement_t> &yk, const Camera &cam ) #else -State::featuresAsMeasurements ( vector<measurement_t> &yk, +State::featuresAsMeasurements ( vector<measurement_t> &yk, const Camera &cam, const Quaterniond &q) #endif { @@ -1071,8 +1071,9 @@ State::featuresAsMeasurements ( vector<measurement_t> &yk, z.id = (*i)->id(); z.source = (*i)->p2x(pbs); z.reflection = (*i)->p2x(pbr); - z.z_type = REFLECTION; - z.patch = (*i)->patch(); + z.z_type = MONO; + z.patch = (*i)->warpedPatch(cam, q); + z.refpatch = (*i)->reflectedPatch(cam, q); // Get ellipse Matrix<double,6,6> s; @@ -1101,13 +1102,35 @@ State::doMeasurements ( vector<measurement_t> &z, Vision &viz, viz.acquireFeatures(cam, newFeatures); } // Measure existing features - vector<measurement_t> currentFeatures, featureZ; + vector<measurement_t> currentFeatures; #if STATESIZE==13 - featuresAsMeasurements(currentFeatures); + featuresAsMeasurements(currentFeatures,cam); #else - featuresAsMeasurements(currentFeatures,q); + featuresAsMeasurements(currentFeatures,cam,q); +#endif + viz.measurements(cam, currentFeatures, z); + +#ifdef DORANSAC +#if STATESIZE==13 + ransacUpdate(z); +#else + ransacUpdate(z,q); +#endif +#else /* ----- not DORANSAC ----- */ + MatrixXd h; +#if STATESIZE==13 + h = H(z); +#else + h = H(q,z); +#endif + MatrixXd S; + S = h*P*h.transpose() + R(z); +#if STATESIZE==13 + kalmanUpdate(h,S,z); +#else + kalmanUpdate(h,S,z,q); +#endif #endif - viz.measurements(cam, currentFeatures, featureZ); // Initialize new features if (features.size()<MINFEATURES) { @@ -1117,6 +1140,15 @@ State::doMeasurements ( vector<measurement_t> &z, Vision &viz, addFeatures(newFeatures, q, hgtmeas); #endif } + vector<measurement_t> updatedFeatures; +#if STATESIZE==13 + featuresAsMeasurements(updatedFeatures, cam); +#else + featuresAsMeasurements(updatedFeatures, cam, q); +#endif + viz.drawMeasurements(z, cam, cv::Scalar(0,0,100), cv::Scalar(0,0,0), false); + viz.drawMeasurements(updatedFeatures, cam, cv::Scalar(0,255,0), cv::Scalar(0,0,0), true); + viz.show(); return ; } /* ----- end of method State::doMeasurements ----- */ |