Demonstrate addFrameSet API

cpp/offline/README.md

@@ -11,7 +11,7 @@ This example shows how to run basic stereo VIO using offline data to mimic real-
 * Clone the submodules: `cd cpp/offline/target && git submodule update --init --recursive`.
 * Build this example using CMake:
 
-```
+```bash
 mkdir target
 cd target
 cmake -DspectacularAI_DIR=<path/to/spectacularai-sdk/lib/cmake/spectacularAI> ..

cpp/offline/input.cpp

@@ -120,28 +120,47 @@ class InputJsonl : public Input {
         data.video1 = nullptr;
         data.accelerometer = nullptr;
         data.gyroscope = nullptr;
+        data.features0.clear();
 
         json j = json::parse(line, nullptr, false); // stream, callback, allow_exceptions
         if (!j.contains("time")) return true;
         data.timestamp = j["time"].get<double>();
 
         if (j.find("sensor") != j.end()) {
-            std::array<double, 3> v = j["sensor"]["values"];
-            *imu = { .x = v[0], .y = v[1], .z = v[2] };
             const std::string sensorType = j["sensor"]["type"];
             if (sensorType == "gyroscope") {
+                std::array<double, 3> v = j["sensor"]["values"];
+                *imu = { .x = v[0], .y = v[1], .z = v[2] };
                 data.gyroscope = imu;
             }
             else if (sensorType == "accelerometer") {
+                std::array<double, 3> v = j["sensor"]["values"];
+                *imu = { .x = v[0], .y = v[1], .z = v[2] };
                 data.accelerometer = imu;
             }
         }
         else if (j.find("frames") != j.end()) {
             json jFrames = j["frames"];
             size_t cameraCount = jFrames.size();
+            if (cameraCount > data.nFrames) data.nFrames = cameraCount;
             assert(cameraCount >= 1);
             int number = j["number"].get<int>();
             for (size_t cameraInd = 0; cameraInd < cameraCount; ++cameraInd) {
+                json jFrame = jFrames[cameraInd];
+                if (cameraInd == 0 && jFrame.contains("features")) {
+                    auto features = jFrame["features"];
+                    for (json::iterator jFeature = features.begin(); jFeature != features.end(); ++jFeature) {
+                        data.features0.push_back({
+                            .id = (*jFeature)["id"],
+                            .point = {
+                                .x = (*jFeature)["point"][0],
+                                .y = (*jFeature)["point"][1]
+                            }
+                        });
+                    }
+                }
+
+                if (jFrame.contains("missingBitmap") && jFrame["missingBitmap"].get<bool>()) continue;
                 std::vector<uint8_t> &video = cameraInd == 0 ? video0 : video1;
                 if (useImageInput) {
                     std::string filePath = stringFormat("%s/frames%zu/%08zu.png",

cpp/offline/input.hpp

@@ -12,10 +12,12 @@ class Input {
         double timestamp = 0.0;
         uint8_t *video0 = nullptr;
         uint8_t *video1 = nullptr;
+        std::vector<spectacularAI::MonocularFeature> features0;
         int width = -1;
         int height = -1;
         std::shared_ptr<spectacularAI::Vector3d> accelerometer;
         std::shared_ptr<spectacularAI::Vector3d> gyroscope;
+        size_t nFrames = 0;
     };
     virtual bool next(Data &data) = 0;
     virtual std::string getConfig() const = 0;

cpp/offline/vio_jsonl.cpp

@@ -1,19 +1,51 @@
+#include "input.hpp"
+
+#include <spectacularAI/vio.hpp>
+
 #include <cassert>
 #include <cstdio>
 #include <fstream>
 #include <iostream>
 #include <memory>
 #include <sstream>
+#include <vector>
 
-#include <spectacularAI/vio.hpp>
+void addFrameSet(spectacularAI::Vio &vio, const Input::Data &data) {
+    if (data.nFrames == 0) return;
+    if (!data.video0 && !data.video0 && data.features0.empty()) return;
 
-#include "input.hpp"
+    spectacularAI::InputFrameSet inputFrameSet;
+    inputFrameSet.timestamp = data.timestamp;
+
+    std::vector<spectacularAI::InputFrame> inputFrames(data.nFrames);
+    inputFrameSet.frames = inputFrames.data();
+    inputFrameSet.nFrames = inputFrames.size();
+    for (size_t i = 0; i < data.nFrames; ++i) {
+        spectacularAI::InputFrame *inputFrame = &inputFrames.at(i);
+        inputFrame->width = data.width;
+        inputFrame->height = data.height;
+        if (i == 0 && !data.features0.empty()) {
+            inputFrame->features = data.features0.data();
+            inputFrame->nFeatures = data.features0.size();
+        }
+        if (i == 0 && data.video0) {
+            inputFrame->data = data.video0;
+            inputFrame->colorFormat = spectacularAI::ColorFormat::GRAY;
+        }
+        if (i == 1 && data.video1) {
+            inputFrame->data = data.video1;
+            inputFrame->colorFormat = spectacularAI::ColorFormat::GRAY;
+        }
+    }
+    vio.addFrameSet(&inputFrameSet);
+}
 
 int main(int argc, char *argv[]) {
     std::vector<std::string> arguments(argv, argv + argc);
     std::unique_ptr<std::ofstream> outputFile;
     std::unique_ptr<Input> input;
     std::string recordingFolder = "";
+    bool useFrameSets = false;
     for (size_t i = 1; i < arguments.size(); ++i) {
         const std::string &argument = arguments.at(i);
         if (argument == "-o") {
@@ -22,6 +54,7 @@ int main(int argc, char *argv[]) {
         }
         else if (argument == "-r") recordingFolder = arguments.at(++i);
         else if (argument == "-i") input = Input::buildJsonl(arguments.at(++i));
+        else if (argument == "-f") useFrameSets = true;
     }
     std::ostream &output = outputFile ? *outputFile : std::cout;
     if (!input) input = Input::buildMock();
@@ -44,7 +77,10 @@ int main(int argc, char *argv[]) {
 
     Input::Data data;
     while (input->next(data)) {
-        if (data.video0 && data.video1) {
+        if (useFrameSets) {
+            addFrameSet(*vio, data);
+        }
+        else if (data.video0 && data.video1) {
             vio->addFrameStereo(data.timestamp, data.width, data.height, data.video0, data.video1,
                 spectacularAI::ColorFormat::GRAY);
         }