Skip to content

Commit c8f3321

Browse files
roym899roym899
committed
Add IMU logging
1 parent 7cef2d8 commit c8f3321

File tree

6 files changed

+214
-24
lines changed

6 files changed

+214
-24
lines changed

‎CMakeLists.txt‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@ set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
1010
set(CMAKE_CXX_STANDARD 17)
1111

1212
include(FetchContent)
13-
FetchContent_Declare(rerun_sdk URL https://build.rerun.io/commit/f3a5ae2/rerun_cpp_sdk.zip)
13+
FetchContent_Declare(rerun_sdk DOWNLOAD_EXTRACT_TIMESTAMP ON URL https://build.rerun.io/commit/2ec5ce7/rerun_cpp_sdk.zip)
1414
FetchContent_MakeAvailable(rerun_sdk)
1515

1616
include(FetchContent)
@@ -20,6 +20,6 @@ FetchContent_MakeAvailable(vrslib)
2020
find_package(Eigen3 REQUIRED)
2121
find_package(OpenCV REQUIRED)
2222

23-
add_executable(log_vrs src/main.cpp src/FramePlayer.cpp src/utils.cpp)
23+
add_executable(log_vrs src/main.cpp src/FramePlayer.cpp src/IMUPlayer.cpp src/utils.cpp)
2424
target_link_libraries(log_vrs Eigen3::Eigen ${OpenCV_LIBS} rerun_sdk vrslib vrs_utils)
2525
target_include_directories(log_vrs PRIVATE src)

‎src/FramePlayer.cpp‎

Lines changed: 9 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -14,16 +14,16 @@
1414
* See the License for the specific language governing permissions and
1515
* limitations under the License.
1616
*/
17-
18-
#include "FramePlayer.h"
19-
#include "utils.h"
20-
21-
#include <vrs/DataLayout.h>
22-
#include <vrs/utils/PixelFrame.h>
2317
#include <iostream>
2418
#include <memory>
2519
#include <sstream>
2620

21+
#include <vrs/DataLayout.h>
22+
#include <vrs/utils/PixelFrame.h>
23+
24+
#include "FramePlayer.h"
25+
#include "utils.h"
26+
2727
namespace rerun_vrs {
2828

2929
struct FrameNumberDataLayout : public vrs::AutoDataLayout {
@@ -32,10 +32,10 @@ namespace rerun_vrs {
3232
vrs::AutoDataLayoutEnd endLayout;
3333
};
3434

35-
RerunFramePlayer::RerunFramePlayer(vrs::StreamId id, std::shared_ptr<rerun::RecordingStream> rec)
35+
FramePlayer::FramePlayer(vrs::StreamId id, std::shared_ptr<rerun::RecordingStream> rec)
3636
: id_{id}, rec_{rec}, entityPath_{add_quotes(id.getName())} {}
3737

38-
bool RerunFramePlayer::onDataLayoutRead(
38+
bool FramePlayer::onDataLayoutRead(
3939
const vrs::CurrentRecord& record, size_t blockIndex, vrs::DataLayout& layout
4040
) {
4141
if (!enabled_)
@@ -69,7 +69,7 @@ namespace rerun_vrs {
6969
return true;
7070
}
7171

72-
bool RerunFramePlayer::onImageRead(
72+
bool FramePlayer::onImageRead(
7373
const vrs::CurrentRecord& record, size_t /*blockIndex*/,
7474
const vrs::ContentBlock& contentBlock
7575
) {

‎src/FramePlayer.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -26,9 +26,9 @@
2626

2727
namespace rerun_vrs {
2828

29-
class RerunFramePlayer : public vrs::utils::VideoRecordFormatStreamPlayer {
29+
class FramePlayer : public vrs::utils::VideoRecordFormatStreamPlayer {
3030
public:
31-
explicit RerunFramePlayer(vrs::StreamId id, std::shared_ptr<rerun::RecordingStream> rec);
31+
explicit FramePlayer(vrs::StreamId id, std::shared_ptr<rerun::RecordingStream> rec);
3232

3333
bool onDataLayoutRead(const vrs::CurrentRecord& r, size_t blockIndex, vrs::DataLayout&)
3434
override;

‎src/IMUPlayer.cpp‎

Lines changed: 136 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,136 @@
1+
/* Modified from https://github.com/facebookresearch/vrs
2+
* and https://github.com/facebookresearch/projectaria_tools
3+
*
4+
* Copyright (c) Meta Platforms, Inc. and affiliates.
5+
*
6+
* Licensed under the Apache License, Version 2.0 (the "License");
7+
* you may not use this file except in compliance with the License.
8+
* You may obtain a copy of the License at
9+
*
10+
* http://www.apache.org/licenses/LICENSE-2.0
11+
*
12+
* Unless required by applicable law or agreed to in writing, software
13+
* distributed under the License is distributed on an "AS IS" BASIS,
14+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15+
* See the License for the specific language governing permissions and
16+
* limitations under the License.
17+
*/
18+
19+
#include <array>
20+
#include <memory>
21+
#include <sstream>
22+
23+
#include <vrs/DataLayout.h>
24+
#include <vrs/utils/PixelFrame.h>
25+
26+
#include "IMUPlayer.h"
27+
#include "utils.h"
28+
29+
namespace rerun_vrs {
30+
31+
struct IMUConfigurationDataLayout : public vrs::AutoDataLayout {
32+
vrs::DataPieceValue<vrs::Bool> hasAccelerometer_{"has_accelerometer"};
33+
vrs::DataPieceValue<vrs::Bool> hasGyroscope_{"has_gyroscope"};
34+
vrs::DataPieceValue<vrs::Bool> hasMagnetometer_{"has_magnetometer"};
35+
36+
vrs::AutoDataLayoutEnd endLayout;
37+
};
38+
39+
struct IMUDataDataLayout : public vrs::AutoDataLayout {
40+
vrs::DataPieceArray<float> accelMSec2{"accelerometer", 3};
41+
vrs::DataPieceArray<float> gyroRadSec{"gyroscope", 3};
42+
vrs::DataPieceArray<float> magTesla{"magnetometer", 3};
43+
44+
vrs::AutoDataLayoutEnd endLayout;
45+
};
46+
47+
IMUPlayer::IMUPlayer(vrs::StreamId id, std::shared_ptr<rerun::RecordingStream> rec)
48+
: id_{id}, rec_{rec}, entityPath_{add_quotes(id.getName())} {}
49+
50+
bool IMUPlayer::onDataLayoutRead(
51+
const vrs::CurrentRecord& record, size_t blockIndex, vrs::DataLayout& layout
52+
) {
53+
if (!enabled_)
54+
return false;
55+
56+
std::ostringstream buffer;
57+
layout.printLayoutCompact(buffer);
58+
const auto& layout_str = buffer.str();
59+
60+
rec_->set_time_seconds("timestamp", record.timestamp);
61+
62+
if (record.recordType == vrs::Record::Type::CONFIGURATION) {
63+
// NOTE this is meta data from the sensor that doesn't change over time and only comes
64+
// in once in the beginning
65+
rec_->log_timeless(
66+
(entityPath_ + "/configuration").c_str(),
67+
rerun::TextDocument(layout_str)
68+
);
69+
70+
// read properties required for logging
71+
auto& config = getExpectedLayout<IMUConfigurationDataLayout>(layout, blockIndex);
72+
hasAccelerometer_ = config.hasAccelerometer_.get();
73+
hasGyroscope_ = config.hasGyroscope_.get();
74+
hasMagnetometer_ = config.hasMagnetometer_.get();
75+
}
76+
77+
if (record.recordType == vrs::Record::Type::DATA) {
78+
auto& data = getExpectedLayout<IMUDataDataLayout>(layout, blockIndex);
79+
80+
if (hasAccelerometer_) {
81+
std::array<float, 3> accelMSec2;
82+
if (data.accelMSec2.get(accelMSec2.data(), accelMSec2.size()))
83+
logAccelerometer(accelMSec2);
84+
}
85+
if (hasGyroscope_) {
86+
std::array<float, 3> gyroRadSec;
87+
if (data.gyroRadSec.get(gyroRadSec.data(), gyroRadSec.size()))
88+
logGyroscope(gyroRadSec);
89+
}
90+
if (hasMagnetometer_) {
91+
std::array<float, 3> magTesla;
92+
if (data.magTesla.get(magTesla.data(), magTesla.size()))
93+
logMagnetometer(magTesla);
94+
}
95+
}
96+
97+
return false;
98+
}
99+
100+
void IMUPlayer::logAccelerometer(const std::array<float, 3>& accelMSec2) {
101+
rec_->log(
102+
(entityPath_ + "/accelerometer").c_str(),
103+
rerun::Arrows3D::from_vectors({rerun::datatypes::Vec3D(accelMSec2)})
104+
);
105+
rec_->log(
106+
(entityPath_ + "/accelerometer/x").c_str(),
107+
rerun::TimeSeriesScalar(accelMSec2[0])
108+
);
109+
rec_->log(
110+
(entityPath_ + "/accelerometer/y").c_str(),
111+
rerun::TimeSeriesScalar(accelMSec2[1])
112+
);
113+
rec_->log(
114+
(entityPath_ + "/accelerometer/z").c_str(),
115+
rerun::TimeSeriesScalar(accelMSec2[2])
116+
);
117+
}
118+
119+
void IMUPlayer::logGyroscope(const std::array<float, 3>& gyroRadSec) {
120+
rec_->log((entityPath_ + "/gyroscope/x").c_str(), rerun::TimeSeriesScalar(gyroRadSec[0]));
121+
rec_->log((entityPath_ + "/gyroscope/y").c_str(), rerun::TimeSeriesScalar(gyroRadSec[1]));
122+
rec_->log((entityPath_ + "/gyroscope/z").c_str(), rerun::TimeSeriesScalar(gyroRadSec[2]));
123+
}
124+
125+
void IMUPlayer::logMagnetometer(const std::array<float, 3>& magTesla) {
126+
rec_->log((entityPath_ + "/magnetometer/x").c_str(), rerun::TimeSeriesScalar(magTesla[0]));
127+
rec_->log((entityPath_ + "/magnetometer/y").c_str(), rerun::TimeSeriesScalar(magTesla[1]));
128+
rec_->log((entityPath_ + "/magnetometer/z").c_str(), rerun::TimeSeriesScalar(magTesla[2]));
129+
}
130+
131+
bool mightContainIMUData(const vrs::StreamId& id) {
132+
return id.getTypeId() == vrs::RecordableTypeId::SlamImuData ||
133+
id.getTypeId() == vrs::RecordableTypeId::SlamMagnetometerData ||
134+
id.getTypeId() == vrs::RecordableTypeId::ImuRecordableClass;
135+
}
136+
} // namespace rerun_vrs

‎src/IMUPlayer.h‎

Lines changed: 52 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,52 @@
1+
/* Modified from https://github.com/facebookresearch/vrs
2+
* and https://github.com/facebookresearch/projectaria_tools
3+
*
4+
* Copyright (c) Meta Platforms, Inc. and affiliates.
5+
*
6+
* Licensed under the Apache License, Version 2.0 (the "License");
7+
* you may not use this file except in compliance with the License.
8+
* You may obtain a copy of the License at
9+
*
10+
* http://www.apache.org/licenses/LICENSE-2.0
11+
*
12+
* Unless required by applicable law or agreed to in writing, software
13+
* distributed under the License is distributed on an "AS IS" BASIS,
14+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15+
* See the License for the specific language governing permissions and
16+
* limitations under the License.
17+
*/
18+
19+
#pragma once
20+
21+
#include <vrs/DataLayout.h>
22+
#include <vrs/RecordFormat.h>
23+
#include <vrs/StreamId.h>
24+
#include <vrs/StreamPlayer.h>
25+
#include <vrs/utils/VideoRecordFormatStreamPlayer.h>
26+
#include <rerun.hpp>
27+
28+
namespace rerun_vrs {
29+
class IMUPlayer : public vrs::RecordFormatStreamPlayer {
30+
public:
31+
explicit IMUPlayer(vrs::StreamId id, std::shared_ptr<rerun::RecordingStream> rec);
32+
33+
bool onDataLayoutRead(const vrs::CurrentRecord& r, size_t blockIndex, vrs::DataLayout&)
34+
override;
35+
36+
private:
37+
void logAccelerometer(const std::array<float, 3>& accelMSec2);
38+
void logGyroscope(const std::array<float, 3>& gyroRadSec);
39+
void logMagnetometer(const std::array<float, 3>& magTesla);
40+
41+
std::shared_ptr<rerun::RecordingStream> rec_;
42+
vrs::StreamId id_;
43+
std::string entityPath_;
44+
bool enabled_{true};
45+
bool hasAccelerometer_;
46+
bool hasGyroscope_;
47+
bool hasMagnetometer_;
48+
};
49+
50+
bool mightContainIMUData(const vrs::StreamId& id);
51+
52+
} // namespace rerun_vrs

‎src/main.cpp‎

Lines changed: 13 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,16 @@
1+
#include <iostream>
2+
#include <set>
3+
#include <vector>
4+
15
#include <vrs/RecordFileReader.h>
26
#include <vrs/RecordFormatStreamPlayer.h>
37
#include <vrs/StreamPlayer.h>
48
#include <eigen3/Eigen/Core>
59
#include <eigen3/Eigen/Dense>
6-
#include <iostream>
710
#include <rerun.hpp>
8-
#include <set>
9-
#include <vector>
1011

1112
#include "FramePlayer.h"
13+
#include "IMUPlayer.h"
1214

1315
int main(int argc, const char* argv[]) {
1416
auto rec = std::make_shared<rerun::RecordingStream>("rerun_example_vrs");
@@ -23,7 +25,8 @@ int main(int argc, const char* argv[]) {
2325

2426
vrs::RecordFileReader reader;
2527
if (reader.openFile(vrsPath) == 0) {
26-
// for each type of stream there is one player that logs the read data to Rerun
28+
// for each type of stream there is one player that logs the read data to
29+
// Rerun
2730
std::vector<std::unique_ptr<vrs::StreamPlayer>> streamPlayers;
2831

2932
const std::set<vrs::StreamId>& streamIds = reader.getStreams();
@@ -32,14 +35,13 @@ int main(int argc, const char* argv[]) {
3235
<< ": ";
3336
if (reader.mightContainImages(id)) {
3437
std::cout << "Handled by FramePlayer" << std::endl;
35-
streamPlayers.emplace_back(std::make_unique<rerun_vrs::RerunFramePlayer>(id, rec));
38+
streamPlayers.emplace_back(std::make_unique<rerun_vrs::FramePlayer>(id, rec));
3639
reader.setStreamPlayer(id, streamPlayers.back().get());
37-
}
38-
/* else if (id.getTypeId() == vrs::RecordableTypeId::ImuRecordableClass) { */
39-
/* /1* streamPlayers.emplace_back(new ImagePlayer()); *1/ */
40-
/* reader.setStreamPlayer(id, streamPlayers.back().get()); */
41-
/* } */
42-
else {
40+
} else if (rerun_vrs::mightContainIMUData(id)) {
41+
std::cout << "Handled by IMUPlayer" << std::endl;
42+
streamPlayers.emplace_back(std::make_unique<rerun_vrs::IMUPlayer>(id, rec));
43+
reader.setStreamPlayer(id, streamPlayers.back().get());
44+
} else {
4345
std::cout << "No player available. Skipped." << std::endl;
4446
}
4547
}

0 commit comments

Comments
 (0)