diff options
Diffstat (limited to 'src/state.cpp')
| -rw-r--r-- | src/state.cpp | 30 |
1 files changed, 23 insertions, 7 deletions
diff --git a/src/state.cpp b/src/state.cpp index 70dc32c..32801f8 100644 --- a/src/state.cpp +++ b/src/state.cpp @@ -172,7 +172,7 @@ State::R ( const vector<measurement_t> &z ) for (int i=0,row=0; i<z.size(); ++i) { Feature *fi; switch ( z[i].z_type ) { - case REFLECTION: + case BOTH: fi = featureById(z[i].id); r.block<6,6>(row,row) = fi->R(); row += 6; @@ -184,6 +184,12 @@ State::R ( const vector<measurement_t> &z ) row += 4; break; + case REFLECTION: + fi = featureById(z[i].id); + r.block<4,4>(row,row) = fi->R().block<4,4>(2,2); + row += 4; + break; + case HEIGHT: r.block<1,1>(row,row) = body->R(); row += 1; @@ -291,7 +297,7 @@ State::innovation( const vector<measurement_t> &z, const Quaterniond &q) for (int i=0,row=0; i<z.size(); ++i) { Feature *fi; measurement_type mt=z[i].z_type; - if (mt==REFLECTION) { + if (mt==BOTH) { fi = featureById(z[i].id); Matrix<double,6,1> hi, zi; hi = fi->h(body->ned(), q); @@ -305,6 +311,13 @@ State::innovation( const vector<measurement_t> &z, const Quaterniond &q) zi = fi->measurement_vector(z[i].source); y.segment<4>(row) = zi-hi; row += 4; + } else if (mt==REFLECTION) { + fi = featureById(z[i].id); + Matrix<double,4,1> hi, zi; + hi = fi->h(body->ned(), q).segment<4>(2); + zi = fi->measurement_vector(z[i].source, z[i].reflection).segment<4>(2); + y.segment<4>(row) = zi-hi; + row += 4; } else if (mt==HEIGHT) { Matrix<double,1,1> zi, hi; zi[0] = z[i].height; @@ -422,6 +435,8 @@ State::blockSI ( const vector<measurement_t> &z, const Quaterniond &q ) #if STATESIZE==13 Quaterniond q = body->qhat(); #endif + fprintf(stderr, "blockSI not implemented\n"); + exit(1); int rows = Hrows(z); MatrixXd SI; SI = MatrixXd::Zero(rows,rows); @@ -498,7 +513,7 @@ State::addFeatures(vector<measurement_t> &F, const Quaterniond &q, double z) if (std::find(blacklist.begin(),blacklist.end(), i->id)!=blacklist.end()) continue; // Create feature Feature *f; - if (i->z_type==REFLECTION) { + if (i->z_type==BOTH) { #ifdef INITDEPTH f = new Feature(i->id, i->source, i->reflection, pos, q, z, i->patch); #else /* ----- not INITDEPTH ----- */ @@ -507,7 +522,7 @@ State::addFeatures(vector<measurement_t> &F, const Quaterniond &q, double z) } else if (i->z_type==MONO) { f = new Feature(i->id, i->source, pos, q, i->patch); } else { - fprintf(stderr, "Measurement type: %d not known, quitting\n", i->z_type); + fprintf(stderr, "Measurement type: %d not supported by addFeatures, quitting\n", i->z_type); exit(1); } if (!f->sane()) { @@ -1088,7 +1103,7 @@ State::featuresAsMeasurements ( vector<measurement_t> &yk, const Camera &cam, z.id = (*i)->id(); z.source = (*i)->p2x(pbs); z.reflection = (*i)->p2x(pbr); - z.z_type = MONO; + z.z_type = BOTH; z.patch = (*i)->warpedPatch(cam, q); z.refpatch = (*i)->reflectedPatch(cam, q); @@ -1163,8 +1178,9 @@ State::doMeasurements ( vector<measurement_t> &z, Vision &viz, #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); + cerr << z.size() << endl; + viz.drawMeasurements(z, cam, cv::Scalar(0,0,200), cv::Scalar(0,0,200), false); + viz.drawMeasurements(updatedFeatures, cam, cv::Scalar(0,255,0), cv::Scalar(0,200,0), true); viz.show(); return ; |