diff options
| -rw-r--r-- | src/main.cpp | 21 | ||||
| -rw-r--r-- | src/main.h | 3 |
2 files changed, 21 insertions, 3 deletions
diff --git a/src/main.cpp b/src/main.cpp index cde65f5..af602c5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -43,8 +43,21 @@ covCallback(const message &msg, State &mu, const Quaterniond &q) } void -imgCallback(const message &msg) +imgCallback(message &msg) { + int id,sx,sy,rx,ry; + 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]); + } + while (fscanf(fin,"%d,%d,%d,%d,%d", &id, &sx, &sy, &rx, &ry)!=EOF) { + printf("id: %d source: (%d, %d) reflection: (%d, %d)\n", id, sx, sy, rx, ry); + } + if (fclose(fin)==EOF) { + err_sys("fclose"); + } + return; } @@ -100,7 +113,6 @@ utmCallback(const message &msg, State &mu) } return; } -#endif /* ----- not USE_ROS ----- */ int parseLine(char *line, message *data) @@ -192,6 +204,7 @@ parseLine(char *line, message *data) exit(1); } } +#endif /* ----- not USE_ROS ----- */ timestamp update_dt(const timestamp t, timestamp *t_old) @@ -215,6 +228,10 @@ update_dt(const timestamp t, timestamp *t_old) int main(int argc, char **argv) { + if (argc!=2) { + fprintf(stderr, "Usage: %s camera\n", argv[0]); + exit(1); + } State mu; Quaterniond qbw; // bias in FRD coordinates @@ -10,6 +10,7 @@ #include "body.h" #include "camera.h" +#include "ourerr.hpp" #include "state.h" #include "types.h" @@ -23,7 +24,7 @@ int parseLine(char *line, message *msg); timestamp update_dt(const timestamp t, timestamp *t_old); void covCallback(const message &msg, Matrix<double,9,9> &P, const Quaterniond &q); -void imgCallback(const message &msg); +void imgCallback(message &msg); void imuCallback(const message &msg, State &mu, const Quaterniond &q, const timestamp dt); void pvaCallback(const message &msg, Matrix<double,9,1> &X, Quaterniond &q); void utmCallback(const message &msg, State &mu); |