summaryrefslogtreecommitdiff
path: root/parse_bundle.cpp
diff options
context:
space:
mode:
authorGuillaume Seguin <guillaume@segu.in>2010-05-28 18:21:18 -0400
committerGuillaume Seguin <guillaume@segu.in>2010-05-28 18:21:18 -0400
commited0412175b47d7335d08b08c42fcb6c60e5e98bb (patch)
treeb866ff3da4af228179c6ad9d039af1ec80536256 /parse_bundle.cpp
downloadmobundleped-master.tar.gz
mobundleped-master.tar.bz2
Initial importHEADmaster
Diffstat (limited to 'parse_bundle.cpp')
-rw-r--r--parse_bundle.cpp545
1 files changed, 545 insertions, 0 deletions
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 <cstdio>
+#include <cstdlib>
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <list>
+#include <map>
+
+#include <vtkSmartPointer.h>
+#include <vtkCellArray.h>
+#include <vtkCellData.h>
+#include <vtkDoubleArray.h>
+#include <vtkPoints.h>
+#include <vtkPolyLine.h>
+#include <vtkPolyData.h>
+#include <vtkPolyDataMapper.h>
+#include <vtkRenderWindow.h>
+#include <vtkRenderWindowInteractor.h>
+#include <vtkCamera.h>
+#include <vtkActor.h>
+#include <vtkRenderer.h>
+#include <vtkProperty.h>
+#include <vtkObjectFactory.h>
+#include <vtkInteractorStyleTrackballCamera.h>
+
+#include <cv.h>
+
+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<Point3d> (0) = M;
+ Mat P = R * M2 + t;
+ P = -P / P.at<double> (2, 0);
+ Point3d p = *P.ptr<Point3d> (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<Point3d> (0);
+ }
+};
+
+struct Keypoint {
+ int key;
+ float x, y;
+};
+
+struct BPoint {
+ Point3d threed;
+ Point3i color;
+ map<int, Keypoint> 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<Point3d> (0) = t;
+ Mat C = - cam->R.t () * cam->t;
+ cam->C = *C.ptr<Point3d> (0);
+ cam->v = - *(cam->R).ptr<Point3d> (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<vtkPoints> points =
+ vtkSmartPointer<vtkPoints>::New ();
+ points->InsertNextPoint (center);
+ points->InsertNextPoint (p0);
+ points->InsertNextPoint (p1);
+ points->InsertNextPoint (p2);
+ points->InsertNextPoint (p3);
+
+ vtkSmartPointer<vtkCellArray> cells =
+ vtkSmartPointer<vtkCellArray>::New ();
+
+ // Camera main frame
+ vtkSmartPointer<vtkPolyLine> polyLine =
+ vtkSmartPointer<vtkPolyLine>::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<vtkPolyLine>::New ();
+ polyLine->GetPointIds ()->SetNumberOfIds (2);
+ polyLine->GetPointIds ()->SetId (0, 0);
+ polyLine->GetPointIds ()->SetId (1, 1);
+ cells->InsertNextCell (polyLine);
+ polyLine = vtkSmartPointer<vtkPolyLine>::New ();
+ polyLine->GetPointIds ()->SetNumberOfIds (2);
+ polyLine->GetPointIds ()->SetId (0, 0);
+ polyLine->GetPointIds ()->SetId (1, 2);
+ cells->InsertNextCell (polyLine);
+ polyLine = vtkSmartPointer<vtkPolyLine>::New ();
+ polyLine->GetPointIds ()->SetNumberOfIds (2);
+ polyLine->GetPointIds ()->SetId (0, 0);
+ polyLine->GetPointIds ()->SetId (1, 3);
+ cells->InsertNextCell (polyLine);
+ polyLine = vtkSmartPointer<vtkPolyLine>::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<BPoint *> & points)
+{
+ vtkPoints *vpoints = vtkPoints::New ();
+ vtkPolyData *poly = vtkPolyData::New ();
+ vtkCellArray *conn = vtkCellArray::New ();
+ for (vector<BPoint *>::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<Point3d> (0) = p;
+ fprintf (stderr, "a %lf %lf %lf\n", P.at<double>(0,0), P.at<double>(1,0), P.at<double>(2,0));
+ fprintf (stderr, "t %lf %lf %lf\n", cam->t.at<double>(0,0), cam->t.at<double>(1,0), cam->t.at<double>(2,0));
+ P = 2 * P - 2 * cam->t;
+ fprintf (stderr, "b %lf %lf %lf\n", P.at<double>(0,0), P.at<double>(1,0), P.at<double>(2,0));
+ P = cam->R.t() * P;
+ fprintf (stderr, "c %lf %lf %lf\n", P.at<double>(0,0), P.at<double>(1,0), P.at<double>(2,0));
+ P = - P;
+ return *P.ptr<Point3d> (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<vtkPolyLine> polyLine =
+ vtkSmartPointer<vtkPolyLine>::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<vtkPolyLine>::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<vtkPolyLine>::New ();
+ polyLine->GetPointIds ()->SetNumberOfIds (2);
+ polyLine->GetPointIds ()->SetId (0, 0);
+ polyLine->GetPointIds ()->SetId (1, 4);
+ cells->InsertNextCell (polyLine);
+ polyLine = vtkSmartPointer<vtkPolyLine>::New ();
+ polyLine->GetPointIds ()->SetNumberOfIds (2);
+ polyLine->GetPointIds ()->SetId (0, 1);
+ polyLine->GetPointIds ()->SetId (1, 5);
+ cells->InsertNextCell (polyLine);
+ polyLine = vtkSmartPointer<vtkPolyLine>::New ();
+ polyLine->GetPointIds ()->SetNumberOfIds (2);
+ polyLine->GetPointIds ()->SetId (0, 2);
+ polyLine->GetPointIds ()->SetId (1, 6);
+ cells->InsertNextCell (polyLine);
+ polyLine = vtkSmartPointer<vtkPolyLine>::New ();
+ polyLine->GetPointIds ()->SetNumberOfIds (2);
+ polyLine->GetPointIds ()->SetId (0, 3);
+ polyLine->GetPointIds ()->SetId (1, 7);
+ cells->InsertNextCell (polyLine);
+
+#ifdef ORIG_OBJECTS
+ polyLine = vtkSmartPointer<vtkPolyLine>::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<vtkPolyLine>::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<vtkPolyLine>::New ();
+ polyLine->GetPointIds ()->SetNumberOfIds (2);
+ polyLine->GetPointIds ()->SetId (0, 0+8);
+ polyLine->GetPointIds ()->SetId (1, 4+8);
+ cells->InsertNextCell (polyLine);
+ polyLine = vtkSmartPointer<vtkPolyLine>::New ();
+ polyLine->GetPointIds ()->SetNumberOfIds (2);
+ polyLine->GetPointIds ()->SetId (0, 1+8);
+ polyLine->GetPointIds ()->SetId (1, 5+8);
+ cells->InsertNextCell (polyLine);
+ polyLine = vtkSmartPointer<vtkPolyLine>::New ();
+ polyLine->GetPointIds ()->SetNumberOfIds (2);
+ polyLine->GetPointIds ()->SetId (0, 2+8);
+ polyLine->GetPointIds ()->SetId (1, 6+8);
+ cells->InsertNextCell (polyLine);
+ polyLine = vtkSmartPointer<vtkPolyLine>::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<Camera *> cams;
+ map<int, list<Object *> > 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<double>(0,0));
+ if (cam->f != 0.)
+ {
+ vtkActor *actor = buildCameraDisplay (cam);
+ // FIXME actors.push_back (actor);
+ renderer->AddActor (actor);
+ list<Object *> image_objects = objects[next_img];
+ int color_id = next_img % 255;
+ for (list<Object *>::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<Camera *> & cams,
+ vector<BPoint *> & points,
+ map<int, list<Object *> > & objects)
+{
+ vtkRenderer *renderer = vtkRenderer::New ();
+ renderer->SetBackground (0.1, 0.1, 0.1);
+ vector<vtkActor *> actors;
+#ifdef SHOW_ALL
+ for (vector<Camera *>::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<double>(0,0));
+ if (cam->f == 0.)
+ continue;
+ vtkActor *actor = buildCameraDisplay (cam);
+ actors.push_back (actor);
+ renderer->AddActor (actor);
+ }
+
+ list<Object *> image_objects;
+ for (map<int, list<Object *> >::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<Object *>::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<Camera *> cams;
+ vector<BPoint *> 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<int, list<Object *> > 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;
+}