diff options
Diffstat (limited to 'src/main.cpp')
| -rw-r--r-- | src/main.cpp | 55 |
1 files changed, 52 insertions, 3 deletions
diff --git a/src/main.cpp b/src/main.cpp index 86d87f4..a30ca6c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -22,15 +22,27 @@ #include "main.h" bool seenutm=false; bool seenpva=false; +Eigen::Matrix<double,9,9> P; #ifdef USE_ROS #else /* ----- not USE_ROS ----- */ void +covCallback(const message &msg, Eigen::Matrix<double,9,9> &P, const Eigen::Quaternion<double> &q) +{ + Eigen::Matrix3d Rbw(q.toRotationMatrix()); + Eigen::Matrix3d Renuned; + Renuned << 0.,1.,0.,1.,0.,0.,0.,0.,-1; + P.block(0,0,3,3) = Renuned*msg.covariance.position*Renuned.transpose(); + P.block(3,3,3,3) = Rbw.transpose()*msg.covariance.velocity*Rbw; +} + +void imgCallback(const message *msg) { return; } + void imuCallback(const message &msg, Eigen::Matrix<double,9,1> &X, const Eigen::Quaternion<double> &q, const timestamp dt) { @@ -113,7 +125,39 @@ parseLine(char *line, message *data) rfn = strsep(&line, ","); sscanf(rfn, "%s", &data->image_names+1); } else if (!strcmp(msg_type,"inscovs")) { + using Eigen::Map; + using Eigen::Matrix3d; + double pos[9]; + double att[9]; + double vel[9]; data->msg_type = INSCOVS; + sscanf(line,"%lf,%lf,%lf,\ + %lf,%lf,%lf,\ + %lf,%lf,%lf,\ + %lf,%lf,%lf,\ + %lf,%lf,%lf,\ + %lf,%lf,%lf,\ + %lf,%lf,%lf,\ + %lf,%lf,%lf,\ + %lf,%lf,%lf", + pos, pos+1, pos+2, + pos+3, pos+4, pos+5, + pos+6, pos+7, pos+8, + att, att+1, att+2, + att+3, att+4, att+5, + att+6, att+7, att+8, + vel, vel+1, vel+2, + vel+3, vel+4, vel+5, + vel+6, vel+7, vel+8); + data->covariance.position << pos[0], pos[1], pos[2], + pos[3], pos[4], pos[5], + pos[6], pos[7], pos[8]; + data->covariance.attitude << att[0], att[1], att[2], + att[3], att[4], att[5], + att[6], att[7], att[8]; + data->covariance.velocity << vel[0], vel[1], vel[2], + vel[3], vel[4], vel[5], + vel[6], vel[7], vel[8]; } else if (!strcmp(msg_type,"inspvas")) { data->msg_type = INSPVAS; sscanf(line,"%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf,%lf", @@ -168,6 +212,7 @@ main(int argc, char **argv) { Eigen::Quaternion<double> qbw; Eigen::Matrix<double,9,1> bodyStateVector; + Eigen::Matrix<double,9,9> P; // bias in FRD coordinates bodyStateVector.segment(6,3) << 0.975*-0.03713532, 0.9*0.01465135, -0.00709229; timestamp dt{0,0}; @@ -176,9 +221,9 @@ main(int argc, char **argv) #ifdef USE_ROS #else /* ----- not USE_ROS ----- */ - using std::cout; - using std::endl; - using std::cerr; +using std::cout; +using std::endl; +using std::cerr; printf("UTM-Zone,UTM-Ch,UTM-North,UTM-East\n"); // Read sensors from file char line[MAXLINE]; @@ -209,6 +254,10 @@ while (scanf("%s", line)!=EOF) { imuCallback(msg, bodyStateVector, qbw, dt); break; + case INSCOVS: + covCallback(msg, P, qbw); + cout << P << endl; + default: break; } /* ----- end switch ----- */ |