From ed0412175b47d7335d08b08c42fcb6c60e5e98bb Mon Sep 17 00:00:00 2001 From: Guillaume Seguin Date: Fri, 28 May 2010 18:21:18 -0400 Subject: Initial import --- parse_bundle.cpp | 545 +++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 545 insertions(+) create mode 100644 parse_bundle.cpp (limited to 'parse_bundle.cpp') diff --git a/parse_bundle.cpp b/parse_bundle.cpp new file mode 100644 index 0000000..b17449d --- /dev/null +++ b/parse_bundle.cpp @@ -0,0 +1,545 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace std; +using namespace cv; + +double Colors[256][3]; + +class Camera { +public: + double f, k1, k2; + Mat R, t; + Point3d C; + Point3d v; + inline Point3d proj (Point3d M) const { + Mat M2 = Mat (3, 1, CV_64FC1); + *M2.ptr (0) = M; + Mat P = R * M2 + t; + P = -P / P.at (2, 0); + Point3d p = *P.ptr (0); + double np = norm (p); + double np2 = np * np; + double r = 1.0 + k1 * np2 + k2 * np2 * np2; + P = f * r * P; + return *P.ptr (0); + } +}; + +struct Keypoint { + int key; + float x, y; +}; + +struct BPoint { + Point3d threed; + Point3i color; + map twod; +}; + +Camera *parseCamera (FILE *f) +{ + Camera *cam = new Camera; + fscanf (f, "%lf %lf %lf", &cam->f, &cam->k1, &cam->k2); + double R[9]; + fscanf (f, "%lf %lf %lf\n%lf %lf %lf\n%lf %lf %lf\n", + &R[0], &R[1], &R[2], &R[3], &R[4], &R[5], &R[6], &R[7], &R[8]); + Point3d t; + fscanf (f, "%lf %lf %lf\n", &t.x, &t.y, &t.z); + Mat Rtemp = Mat (3, 3, CV_64FC1, R); + cam->R = Rtemp.clone (); + cam->t = Mat (3, 1, CV_64FC1); + *cam->t.ptr (0) = t; + Mat C = - cam->R.t () * cam->t; + cam->C = *C.ptr (0); + cam->v = - *(cam->R).ptr (2); + double nv = 2 * norm (cam->v); + cam->v = Point3d (cam->v.x / nv, cam->v.y / nv, cam->v.z / nv); + return cam; +} + +BPoint *parsePoint (FILE *f) +{ + BPoint *p = new BPoint; + double threed[3]; + fscanf (f, "%lf %lf %lf", &threed[0], &threed[1], &threed[2]); + int color[3]; + fscanf (f, "%d %d %d", &color[0], &color[1], &color[2]); + p->threed = Point3d (threed[0], threed[1], threed[2]); + p->color = Point3d (color[0], color[1], color[2]); + int numVisible; + fscanf (f, "%d", &numVisible); + for (int i = 0; i < numVisible; ++i) + { + int view; + Keypoint kp; + fscanf (f, "%d %d %f %f", &view, &kp.key, &kp.x, &kp.y); + p->twod[view] = kp; + } + return p; +} + +struct Object { + string type; + Point3d position; + double rotation[4]; + double score; + Point3d bounding[8]; +}; + +Object *parseObject (FILE *f) +{ + Object *o = new Object; + char buf[300]; + double dbuf[4]; + fscanf (f, "%s [", buf); + o->type = string (buf); + fscanf (f, "%lf %lf %lf] [", &dbuf[0], &dbuf[1], &dbuf[2]); + o->position = Point3d (dbuf[0], dbuf[1], dbuf[2]); + fscanf (f, "%lf %lf %lf %lf]", &dbuf[0], &dbuf[1], &dbuf[2], &dbuf[3]); + memcpy (o->rotation, dbuf, sizeof (double) * 4); + o->position = Point3d (dbuf[0], dbuf[1], dbuf[2]); + fscanf (f, "%lf [", &dbuf[0]); + o->score = dbuf[0]; + for(int i=0; i < 8; i++) { + fscanf (f, "%lf %lf %lf] [", &dbuf[0], &dbuf[1], &dbuf[2]); + o->bounding[i] = Point3d (dbuf[0], dbuf[1], dbuf[2]); + } + return o; +} + +vtkActor *buildCameraDisplay (Camera *cam) +{ + double center[3] = {cam->C.x, cam->C.y, cam->C.z}; + Vec3d p = cam->C + cam->v; + + double p0[3] = {p[0], p[1], p[2]}; + double p1[3] = {p[0], p[1], p[2]}; + double p2[3] = {p[0], p[1], p[2]}; + double p3[3] = {p[0], p[1], p[2]}; + + vtkSmartPointer points = + vtkSmartPointer::New (); + points->InsertNextPoint (center); + points->InsertNextPoint (p0); + points->InsertNextPoint (p1); + points->InsertNextPoint (p2); + points->InsertNextPoint (p3); + + vtkSmartPointer cells = + vtkSmartPointer::New (); + + // Camera main frame + vtkSmartPointer polyLine = + vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (5); + polyLine->GetPointIds ()->SetId (0, 1); + polyLine->GetPointIds ()->SetId (1, 2); + polyLine->GetPointIds ()->SetId (2, 3); + polyLine->GetPointIds ()->SetId (3, 4); + polyLine->GetPointIds ()->SetId (4, 1); + cells->InsertNextCell (polyLine); + + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (2); + polyLine->GetPointIds ()->SetId (0, 0); + polyLine->GetPointIds ()->SetId (1, 1); + cells->InsertNextCell (polyLine); + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (2); + polyLine->GetPointIds ()->SetId (0, 0); + polyLine->GetPointIds ()->SetId (1, 2); + cells->InsertNextCell (polyLine); + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (2); + polyLine->GetPointIds ()->SetId (0, 0); + polyLine->GetPointIds ()->SetId (1, 3); + cells->InsertNextCell (polyLine); + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (2); + polyLine->GetPointIds ()->SetId (0, 0); + polyLine->GetPointIds ()->SetId (1, 4); + cells->InsertNextCell (polyLine); + + vtkPolyData *polyData = vtkPolyData::New (); + polyData->SetPoints (points); + polyData->SetLines (cells); + + vtkPolyDataMapper *mapper = vtkPolyDataMapper::New (); + mapper->SetInput (polyData); + + vtkActor *actor = vtkActor::New (); + actor->SetMapper (mapper); + actor->GetProperty()->SetColor (Colors[255][0], + Colors[255][1], + Colors[255][2]); + + return actor; +} + +vtkActor *buildPointsCloud (vector & points) +{ + vtkPoints *vpoints = vtkPoints::New (); + vtkPolyData *poly = vtkPolyData::New (); + vtkCellArray *conn = vtkCellArray::New (); + for (vector::iterator it = points.begin (); it != points.end (); ++it) + { + BPoint *bp = *it; + double p[3] = {bp->threed.x, bp->threed.y, bp->threed.z}; + vpoints->InsertNextPoint (p); + } + + for (int i = 0; i < vpoints->GetNumberOfPoints (); i++) + conn->InsertNextCell (i); + + poly->SetPoints (vpoints); + poly->SetVerts (conn); + vpoints->Delete (); + conn->Delete (); + + vtkPolyDataMapper *mapper = vtkPolyDataMapper::New (); + mapper->SetInput (poly); + + vtkActor *actor = vtkActor::New (); + actor->SetMapper (mapper); + return actor; +} + +Point3d world_from_cam (Camera *cam, Point3d p) +{ + Mat P = Mat (3, 1, CV_64FC1); + *P.ptr (0) = p; + fprintf (stderr, "a %lf %lf %lf\n", P.at(0,0), P.at(1,0), P.at(2,0)); + fprintf (stderr, "t %lf %lf %lf\n", cam->t.at(0,0), cam->t.at(1,0), cam->t.at(2,0)); + P = 2 * P - 2 * cam->t; + fprintf (stderr, "b %lf %lf %lf\n", P.at(0,0), P.at(1,0), P.at(2,0)); + P = cam->R.t() * P; + fprintf (stderr, "c %lf %lf %lf\n", P.at(0,0), P.at(1,0), P.at(2,0)); + P = - P; + return *P.ptr (0); +} + +vtkActor *buildObjectDisplay (Camera *cam, Object *object, int color_id) +{ + vtkPoints *vpoints = vtkPoints::New (); + vtkPolyData *poly = vtkPolyData::New (); + vtkCellArray *cells = vtkCellArray::New (); + + for (int i = 0; i < 8; ++i) + { + Point3d w = world_from_cam (cam, object->bounding[i]); + double p[3] = {w.x, w.y, w.z}; + fprintf (stderr, "%lf %lf %lf\n", w.x, w.y, w.z); + vpoints->InsertNextPoint (p); + } +#ifdef ORIG_OBJECTS + for (int i = 0; i < 8; ++i) + { + Point3d w = object->bounding[i]; + double p[3] = {w.x, w.y, w.z}; + fprintf (stderr, "%lf %lf %lf\n", w.x, w.y, w.z); + vpoints->InsertNextPoint (p); + } +#endif + + vtkSmartPointer polyLine = + vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (5); + polyLine->GetPointIds ()->SetId (0, 0); + polyLine->GetPointIds ()->SetId (1, 1); + polyLine->GetPointIds ()->SetId (2, 3); + polyLine->GetPointIds ()->SetId (3, 2); + polyLine->GetPointIds ()->SetId (4, 0); + cells->InsertNextCell (polyLine); + + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (5); + polyLine->GetPointIds ()->SetId (0, 4); + polyLine->GetPointIds ()->SetId (1, 5); + polyLine->GetPointIds ()->SetId (2, 7); + polyLine->GetPointIds ()->SetId (3, 6); + polyLine->GetPointIds ()->SetId (4, 4); + cells->InsertNextCell (polyLine); + + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (2); + polyLine->GetPointIds ()->SetId (0, 0); + polyLine->GetPointIds ()->SetId (1, 4); + cells->InsertNextCell (polyLine); + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (2); + polyLine->GetPointIds ()->SetId (0, 1); + polyLine->GetPointIds ()->SetId (1, 5); + cells->InsertNextCell (polyLine); + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (2); + polyLine->GetPointIds ()->SetId (0, 2); + polyLine->GetPointIds ()->SetId (1, 6); + cells->InsertNextCell (polyLine); + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (2); + polyLine->GetPointIds ()->SetId (0, 3); + polyLine->GetPointIds ()->SetId (1, 7); + cells->InsertNextCell (polyLine); + +#ifdef ORIG_OBJECTS + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (5); + polyLine->GetPointIds ()->SetId (0, 0+8); + polyLine->GetPointIds ()->SetId (1, 1+8); + polyLine->GetPointIds ()->SetId (2, 3+8); + polyLine->GetPointIds ()->SetId (3, 2+8); + polyLine->GetPointIds ()->SetId (4, 0+8); + cells->InsertNextCell (polyLine); + + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (5); + polyLine->GetPointIds ()->SetId (0, 4+8); + polyLine->GetPointIds ()->SetId (1, 5+8); + polyLine->GetPointIds ()->SetId (2, 7+8); + polyLine->GetPointIds ()->SetId (3, 6+8); + polyLine->GetPointIds ()->SetId (4, 4+8); + cells->InsertNextCell (polyLine); + + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (2); + polyLine->GetPointIds ()->SetId (0, 0+8); + polyLine->GetPointIds ()->SetId (1, 4+8); + cells->InsertNextCell (polyLine); + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (2); + polyLine->GetPointIds ()->SetId (0, 1+8); + polyLine->GetPointIds ()->SetId (1, 5+8); + cells->InsertNextCell (polyLine); + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (2); + polyLine->GetPointIds ()->SetId (0, 2+8); + polyLine->GetPointIds ()->SetId (1, 6+8); + cells->InsertNextCell (polyLine); + polyLine = vtkSmartPointer::New (); + polyLine->GetPointIds ()->SetNumberOfIds (2); + polyLine->GetPointIds ()->SetId (0, 3+8); + polyLine->GetPointIds ()->SetId (1, 7+8); + cells->InsertNextCell (polyLine); +#endif + + poly->SetPoints (vpoints); + poly->SetLines (cells); + vpoints->Delete (); + cells->Delete (); + + vtkPolyDataMapper *mapper = vtkPolyDataMapper::New (); + mapper->SetInput (poly); + + vtkActor *actor = vtkActor::New (); + actor->SetMapper (mapper); + actor->GetProperty()->SetColor (Colors[color_id][0], + Colors[color_id][1], + Colors[color_id][2]); + return actor; +} + +// Define interaction style +class KeyPressInteractorStyle : public vtkInteractorStyleTrackballCamera +{ + public: + int next_img; + vtkRenderer *renderer; + vtkRenderWindow *renderWindow; + vector cams; + map > objects; + static KeyPressInteractorStyle* New(); + vtkTypeRevisionMacro(KeyPressInteractorStyle, vtkInteractorStyleTrackballCamera); + + virtual void OnKeyPress() + { +#ifndef SHOW_ALL + if (next_img < cams.size ()) + { + Camera *cam = cams[next_img]; + fprintf (stderr, "f %lf\n", cam->f); + fprintf (stderr, "t %lf\n", cam->t.at(0,0)); + if (cam->f != 0.) + { + vtkActor *actor = buildCameraDisplay (cam); + // FIXME actors.push_back (actor); + renderer->AddActor (actor); + list image_objects = objects[next_img]; + int color_id = next_img % 255; + for (list::iterator it = image_objects.begin (); + it != image_objects.end (); ++it) + { + Object *o = *it; + vtkActor *actor = buildObjectDisplay (cam, o, color_id); + // FIXME actors.push_back (actor); + renderer->AddActor (actor); + } + renderWindow->Render (); + } + } + next_img++; +#endif + vtkInteractorStyleTrackballCamera::OnKeyPress(); + } + +}; +vtkCxxRevisionMacro(KeyPressInteractorStyle, "$Revision: 1.1 $"); +vtkStandardNewMacro(KeyPressInteractorStyle); + +void display (vector & cams, + vector & points, + map > & objects) +{ + vtkRenderer *renderer = vtkRenderer::New (); + renderer->SetBackground (0.1, 0.1, 0.1); + vector actors; +#ifdef SHOW_ALL + for (vector::iterator it = cams.begin (); it != cams.end (); ++it) + { + Camera *cam = *it; + fprintf (stderr, "f %lf\n", cam->f); + fprintf (stderr, "t %lf\n", cam->t.at(0,0)); + if (cam->f == 0.) + continue; + vtkActor *actor = buildCameraDisplay (cam); + actors.push_back (actor); + renderer->AddActor (actor); + } + + list image_objects; + for (map >::iterator img_it = objects.begin (); + img_it != objects.end (); ++img_it) + { + int cam_id = img_it->first; + int color_id = cam_id % 255; + image_objects = img_it->second; + for (list::iterator it = image_objects.begin (); + it != image_objects.end (); ++it) + { + Object *o = *it; + vtkActor *actor = buildObjectDisplay (cams[cam_id], o, color_id); + actors.push_back (actor); + renderer->AddActor (actor); + } + } +#endif + + vtkActor *actor = buildPointsCloud (points); + actors.push_back (actor); + renderer->AddActor (actor); + + vtkRenderWindow *renWin = vtkRenderWindow::New (); + renWin->AddRenderer (renderer); + renWin->SetSize (800, 600); + + vtkRenderWindowInteractor *iren = vtkRenderWindowInteractor::New (); + iren->SetRenderWindow (renWin); + + KeyPressInteractorStyle *style = + KeyPressInteractorStyle::New (); + style->cams = cams; + style->objects = objects; + style->next_img = 0; + style->renderer = renderer; + style->renderWindow = renWin; + iren->SetInteractorStyle (style); + + iren->Initialize (); + iren->Start (); + + // [...] mainloop is going on there + + // Cleanup + renderer->Delete (); + renWin->Delete (); + iren->Delete (); + style->Delete (); +} + +void fillColors (void) +{ + for (int i=0; i<256; i++) { + int r = i; + r = (((r * 214013L + 2531011L) >> 16) & 32767); + int a = 192 + r%64; + r = (((r * 214013L + 2531011L) >> 16) & 32767); + int b = 64 + r%128; + r = (((r * 214013L + 2531011L) >> 16) & 32767); + int c = r%64; + for(int x=0; x<100; x++) if( (r = (((r * 214013L + 2531011L) >> 16) & 32767)) % 2 ) swap(a,b); else swap(a,c); + Colors[i][0] = ((double) a) / 255.; + Colors[i][1] = ((double) b) / 255.; + Colors[i][2] = ((double) c) / 255.; + } +} + +int main (int argc, char **argv) +{ + if (argc < 3) + { + fprintf (stderr, "Usage : %s bundle.out moped.out\n", argv[0]); + exit (-2); + } + fillColors (); + string bundleOutPath = argv[1]; + FILE *bundleOut = fopen (bundleOutPath.c_str (), "r"); + fscanf (bundleOut, "# Bundle file v0.3"); + int nCams, nPoints; + fscanf (bundleOut, "%d %d", &nCams, &nPoints); + vector cams; + vector points; + for (int nCam = 0; nCam < nCams; ++nCam) + { + Camera *cam = parseCamera (bundleOut); + cams.push_back (cam); + } + for (int nPoint = 0; nPoint < nPoints; ++nPoint) + { + BPoint *point = parsePoint (bundleOut); + points.push_back (point); + } + fclose (bundleOut); + string mopedOutPath = argv[2]; + FILE *mopedOut = fopen (mopedOutPath.c_str (), "r"); + char path[300]; + int numObjects; + map > objects; + while (EOF != fscanf (mopedOut, "%s %d", path, &numObjects)) + { + int image_id; + sscanf (path, "../dataset2/image%d.jpg", &image_id); + for (int object = 0; object < numObjects; ++object) + { + Object *object = parseObject (mopedOut); + objects[image_id].push_back (object); + } + } + fclose (mopedOut); + display (cams, points, objects); + return 0; +} -- cgit v1.1