Merge branch 'logger'

CMakeLists.txt

@@ -19,6 +19,8 @@ add_executable(LoggerRealSense
 				CameraInterface.h
 				RealSenseInterface.cpp
 				RealSenseInterface.h
+				Logger.cpp
+				Logger.h
 )
 
 target_link_libraries(LoggerRealSense

CameraInterface.h

@@ -13,12 +13,18 @@
 class CameraInterface
 {
     public:
+      CameraInterface(int width,int height,int fps)
+        : width(width),height(height),fps(fps)
+      {
+      }
+
       virtual ~CameraInterface() {}
 
       virtual bool ok() = 0;
       virtual std::string error() = 0;
       virtual float depthScale() = 0;
 
+      const int width,height,fps;
       static const int numBuffers = 10;
       std::atomic<int> latestDepthIndex;
       std::pair<std::pair<uint8_t *,uint8_t *>,int64_t> frameBuffers[numBuffers];

Logger.cpp

@@ -0,0 +1,90 @@
+#include "Logger.h"
+
+#include <iostream>
+
+using std::cout;
+
+Logger::Logger(string outDir,std::shared_ptr<CameraInterface> cam)
+  :
+  doWrite(false),
+  outDir(outDir),
+  fileId(0),
+  writeThread(nullptr),
+  cam(cam)
+{
+}
+
+Logger::~Logger()
+{
+  if(doWrite)
+  {
+    stopWriting();
+  }
+}
+
+void Logger::startWriting()
+{
+  cout << "Starting writing.\n";
+  doWrite = true;
+  nFrames = 0;
+
+  string fn = outDir + "/seq" + std::to_string(fileId++) + ".klg";
+  file.open(fn,std::ios::binary);
+  if(!file.is_open())
+  {
+    cout << "Could not open file: " << fn << "\n";
+    doWrite = false;
+    return;
+  }
+
+  file.write((char*)&nFrames,sizeof(int32_t));
+  writeThread = new std::thread(&Logger::write,this);
+}
+
+void Logger::stopWriting()
+{
+  cout << "Stopping writing.\n";
+  doWrite = false;
+  writeThread->join();
+  delete writeThread;
+  writeThread = nullptr;
+
+  file.seekp(0);
+  file.write((char*)&nFrames,sizeof(int32_t));
+
+  file.close();
+  file.clear();
+}
+
+void Logger::write()
+{
+  static int lastWrittenBufferIdx = -1;
+  while(doWrite)
+  {
+    std::this_thread::sleep_for(std::chrono::microseconds(1000));
+
+    int lastDepth = cam->latestDepthIndex;
+    if(lastDepth == -1)
+      continue;
+
+    int bufferIdx = lastDepth % CameraInterface::numBuffers;
+    if(bufferIdx == lastWrittenBufferIdx)
+      continue;
+
+    const void *depthData = cam->frameBuffers[bufferIdx].first.first;
+    const void *rgbData = cam->frameBuffers[bufferIdx].first.second;
+
+    int64_t currTimestamp = cam->frameBuffers[bufferIdx].second;
+    int32_t depthSize = cam->width * cam->height * sizeof(uint16_t);
+    int32_t rgbSize = cam->width * cam->height * 3 * sizeof(uint8_t);
+
+    file.write((char*)&currTimestamp,sizeof(int64_t));
+    file.write((char*)&depthSize,sizeof(int32_t));
+    file.write((char*)&rgbSize,sizeof(int32_t));
+    file.write((char*)depthData,depthSize);
+    file.write((char*)rgbData,rgbSize);
+
+    lastWrittenBufferIdx = bufferIdx;
+    nFrames++;
+  }
+}

Logger.h

@@ -0,0 +1,50 @@
+#pragma once
+
+#include <string>
+#include <atomic>
+#include <memory>
+#include <cstdint>
+#include <fstream>
+#include <thread>
+
+#include "CameraInterface.h"
+
+using std::string;
+
+/**
+* Format is:
+* int32_t at file beginning for frame count
+*
+* For each frame:
+* int64_t: timestamp
+* int32_t: depthSize
+* int32_t: imageSize
+* depthSize * unsigned char: depth_compress_buf
+* imageSize * unsigned char: encodedImage->data.ptr
+*/
+
+class Logger
+{
+public:
+  Logger(string outDir,std::shared_ptr<CameraInterface> cam);
+  ~Logger();
+
+  bool isWriting() const
+  {
+    return doWrite;
+  }
+
+  void startWriting();
+  void stopWriting();
+
+private:
+  void write();
+
+  std::atomic<bool> doWrite;
+  string outDir;
+  int32_t nFrames;
+  int fileId;
+  std::ofstream file;
+  std::thread *writeThread;
+  std::shared_ptr<CameraInterface> cam;
+};

README.md

@@ -1,3 +1,6 @@
+How to run:
+Press button R to start recording and press it again to stop.
+
 Dependencies:
 
 * librealsense

RealSenseInterface.cpp

@@ -2,9 +2,8 @@
 #include <functional>
 
 RealSenseInterface::RealSenseInterface(int inWidth,int inHeight,int inFps)
-  : width(inWidth),
-  height(inHeight),
-  fps(inFps),
+  : 
+  CameraInterface(inWidth,inHeight,inFps),
   dev(nullptr),
   initSuccessful(true)
 {

RealSenseInterface.h

@@ -16,8 +16,6 @@ class RealSenseInterface : public CameraInterface
   RealSenseInterface(int width = 640,int height = 480,int fps = 30);
   virtual ~RealSenseInterface();
 
-  const int width,height,fps;
-
   virtual bool ok()
   {
     return initSuccessful;

main.cpp

@@ -1,6 +1,6 @@
 #include <iostream>
 #include <memory>
-#include <vector>
+#include <string>
 #include <cstdint>
 
 #include "GL/glut.h"
@@ -9,14 +9,16 @@
 
 #include "CameraInterface.h"
 #include "RealSenseInterface.h"
+#include "Logger.h"
 
 using std::cout;
-using std::vector;
+using std::string;
 
 const static int width = 640;
 const static int height = 480;
 const static int fps = 30;
-std::unique_ptr<CameraInterface> cam;
+std::shared_ptr<CameraInterface> cam;
+std::unique_ptr<Logger> logger;
 
 void displayCallback()
 {
@@ -52,6 +54,10 @@ void idleCallback()
 void endApp()
 {
   glutLeaveMainLoop();
+  if(logger->isWriting())
+  {
+    logger->stopWriting();
+  }
 }
 
 void keyboardCallback(unsigned char keyPressed,int mouseX,int mouseY)
@@ -61,19 +67,25 @@ void keyboardCallback(unsigned char keyPressed,int mouseX,int mouseY)
   case 27:
     endApp();
     break;
-  /*case 'r':
-    if(!lastPressStateKeyR)
-      restartGame();
-    lastPressStateKeyR = true;
-    break;*/
+  case 'r':
+    if(logger->isWriting())
+      logger->stopWriting();
+    else
+      logger->startWriting();
+    break;
   default:
     break;
   }
 }
 
 int main(int argc,char *argv[])
 {
-  cam = std::make_unique<RealSenseInterface>(width,height,fps);
+  string outDir = "";
+  if(argc > 1)
+    outDir = argv[1];
+
+  cam = std::make_shared<RealSenseInterface>(width,height,fps);
+  logger = std::make_unique<Logger>(outDir,cam);
 
   glutInit(&argc,argv);