summaryrefslogtreecommitdiff
path: root/src/state.cpp
diff options
context:
space:
mode:
authorMartin Miller2017-03-25 18:47:40 -0500
committerMartin Miller2017-03-25 18:47:40 -0500
commitbd36055542b49b0bb7611ec9c320f7bbabaf6c27 (patch)
treedec90e5991e787c3bf5cd5c74c22f6c5fe66bbeb /src/state.cpp
parent8ee0614580b266aa154ce926b2bac7d0e9159578 (diff)
downloadrefslam-bd36055542b49b0bb7611ec9c320f7bbabaf6c27.zip
refslam-bd36055542b49b0bb7611ec9c320f7bbabaf6c27.tar.gz
Add State::R() method, refactor H()
Method to compute R for given vector of z measurements. Added a method State::Hrows(z) to return number of rows in H(z).
Diffstat (limited to 'src/state.cpp')
-rw-r--r--src/state.cpp76
1 files changed, 62 insertions, 14 deletions
diff --git a/src/state.cpp b/src/state.cpp
index 281fac3..243c34b 100644
--- a/src/state.cpp
+++ b/src/state.cpp
@@ -53,6 +53,46 @@ State::H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurem
{
MatrixXd h;
// Determine the size of H
+ int cols = 9 + 3*features.size();
+ int rows = Hrows(z);
+ h = MatrixXd::Zero(rows,cols);
+ for (int i=0,row=0; i<z.size(); ++i) {
+ switch ( z[i].z_type ) {
+ int col;
+ case REFLECTION:
+ col = rowById(z[i].id);
+ Feature *fi;
+ fi = featureById(z[i].id);
+ if (col==-1) {
+ fprintf(stderr, "Feature %d not found, quitting.\n", z[i].id);
+ exit(1);
+ }
+
+ h.block<6,9>(row,0) = fi->Hx(pos,q);
+ h.block<6,3>(row,col) = fi->Hy(pos,q);
+ row += 6;
+ break;
+
+ case MONO:
+ fprintf(stderr, "mono points not supported.\n");
+ exit(1);
+ break;
+
+ case HEIGHT:
+ h.block<1,9>(row,0) = body->H();
+ row += 1;
+ break;
+
+ default:
+ break;
+ } /* ----- end switch ----- */
+ }
+ return h;
+} /* ----- end of method State::H ----- */
+
+int
+State::Hrows ( const std::vector<measurement_t> &z)
+{
int rows = 0;
for (auto i=z.begin(); i!=z.end(); ++i) {
switch ( i->z_type ) {
@@ -72,22 +112,22 @@ State::H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurem
break;
} /* ----- end switch ----- */
}
- int cols = 9 + 3*features.size();
- h = MatrixXd::Zero(rows,cols);
+ return rows ;
+} /* ----- end of method State::Hrows ----- */
+
+MatrixXd
+State::R ( const std::vector<measurement_t> &z )
+{
+ int rows = Hrows(z);
+ MatrixXd r;
+ r = MatrixXd::Zero(rows,rows);
+
for (int i=0,row=0; i<z.size(); ++i) {
switch ( z[i].z_type ) {
- int col;
case REFLECTION:
- col = rowById(z[i].id);
Feature *fi;
fi = featureById(z[i].id);
- if (col==-1) {
- fprintf(stderr, "Feature %d not found, quitting.\n", z[i].id);
- exit(1);
- }
-
- h.block<6,9>(row,0) = fi->Hx(pos,q);
- h.block<6,3>(row,col) = fi->Hy(pos,q);
+ r.block<6,6>(row,row) = fi->R();
row += 6;
break;
@@ -97,7 +137,7 @@ State::H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurem
break;
case HEIGHT:
- h.block<1,9>(row,0) = body->H();
+ r.block<1,1>(row,row) = body->R();
row += 1;
break;
@@ -105,14 +145,22 @@ State::H ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurem
break;
} /* ----- end switch ----- */
}
- return h;
-} /* ----- end of method State::H ----- */
+
+ return r ;
+} /* ----- end of method State::R ----- */
void
State::update ( const Vector3d &pos, const Quaterniond &q, const std::vector<measurement_t> &z )
{
MatrixXd h;
h = H(pos,q,z);
+
+#ifdef FULLS
+ MatrixXd S;
+ S = h*P*h.transpose() + R(z);
+#else /* ----- not FULLS ----- */
+ <+ELSE PART+>
+#endif /* ----- not FULLS ----- */
/*
Matrix<double,1,1> S;
S=body->S(Pxx());