diff --git a/cpp/offline/README.md b/cpp/offline/README.md index b42eeba..aadfaf9 100644 --- a/cpp/offline/README.md +++ b/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= .. diff --git a/cpp/offline/input.cpp b/cpp/offline/input.cpp index 2561d80..fb9a4cd 100644 --- a/cpp/offline/input.cpp +++ b/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(); if (j.find("sensor") != j.end()) { - std::array 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 v = j["sensor"]["values"]; + *imu = { .x = v[0], .y = v[1], .z = v[2] }; data.gyroscope = imu; } else if (sensorType == "accelerometer") { + std::array 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(); 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()) continue; std::vector &video = cameraInd == 0 ? video0 : video1; if (useImageInput) { std::string filePath = stringFormat("%s/frames%zu/%08zu.png", diff --git a/cpp/offline/input.hpp b/cpp/offline/input.hpp index 27b01e6..9cd07f0 100644 --- a/cpp/offline/input.hpp +++ b/cpp/offline/input.hpp @@ -12,10 +12,12 @@ class Input { double timestamp = 0.0; uint8_t *video0 = nullptr; uint8_t *video1 = nullptr; + std::vector features0; int width = -1; int height = -1; std::shared_ptr accelerometer; std::shared_ptr gyroscope; + size_t nFrames = 0; }; virtual bool next(Data &data) = 0; virtual std::string getConfig() const = 0; diff --git a/cpp/offline/vio_jsonl.cpp b/cpp/offline/vio_jsonl.cpp index 895e042..0305b19 100644 --- a/cpp/offline/vio_jsonl.cpp +++ b/cpp/offline/vio_jsonl.cpp @@ -1,19 +1,51 @@ +#include "input.hpp" + +#include + #include #include #include #include #include #include +#include -#include +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 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 arguments(argv, argv + argc); std::unique_ptr outputFile; std::unique_ptr 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); }