diff options
Diffstat (limited to 'src/main.cpp')
| -rw-r--r-- | src/main.cpp | 17 |
1 files changed, 14 insertions, 3 deletions
diff --git a/src/main.cpp b/src/main.cpp index af602c5..390a57e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -43,8 +43,9 @@ covCallback(const message &msg, State &mu, const Quaterniond &q) } void -imgCallback(message &msg) +imgCallback(message &msg, State &mu, Camera &cam, const Quaterniond &q) { + std::vector<measurement_t> z; int id,sx,sy,rx,ry; strcat(msg.image_names[0],".txt"); FILE *fin; @@ -52,11 +53,21 @@ imgCallback(message &msg) 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); + measurement_t m; + m.id = id; + // Points in the image frame + Vector3d xi, xir; + xi << sx, sy, 1.; + xir << rx, ry, 1.; + // Points in the camera frame + m.source = cam.img2body(xi); + m.reflection = cam.img2body(xir); + z.push_back(m); } if (fclose(fin)==EOF) { err_sys("fclose"); } + mu.feature_update(z,q); return; } @@ -261,7 +272,7 @@ while (scanf("%s", line)!=EOF) { break; case IMG: - imgCallback(msg); + imgCallback(msg, mu, cam, qbw); break; case INSPVAS: |