Skip to content

Commit

Permalink
Split IMU Cam
Browse files Browse the repository at this point in the history
  • Loading branch information
@hungdche
hungdche committed Jan 25, 2023
1 parent 4d78923 commit d2132c0
Showing 1 changed file with 34 additions and 35 deletions.
69 changes: 34 additions & 35 deletions src/plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,11 @@ class orb_slam3 : public plugin {
: plugin{name_, pb_}
, sb{pb->lookup_impl<switchboard>()}
, _m_pose{sb->get_writer<pose_type>("slow_pose")}
// , root_path{getenv("ORB_SLAM_ROOT")}
// , vocab_path{root_path / "Vocabulary" / "ORBvoc.txt"}
, _m_imu_integrator_input{sb->get_writer<imu_integrator_input>("imu_integrator_input")}
, root_path{getenv("ORB_SLAM_ROOT")}
, vocab_path{root_path / "Vocabulary" / "ORBvoc.txt"}
, _m_cam{sb->get_buffered_reader<cam_type>("cam")}
, vocab_path{"/home/henrydc/tinker/ILLIXR/ILLIXR-Repos/ORB_SLAM3/Vocabulary/ORBvoc.txt"}
{

// set initial slow pose
Expand All @@ -48,8 +50,11 @@ class orb_slam3 : public plugin {

// set setting path and initialize ORB_SLAM3
#ifdef STEREO_IMU
setting_path = root_path / "Examples" / "Stereo-Inertial" / "ETH3D.yaml";
// setting_path = root_path / "Examples" / "Stereo-Inertial" / "ETH3D.yaml";
setting_path = "/home/henrydc/tinker/ILLIXR/ILLIXR-Repos/ORB_SLAM3/Examples/Stereo-Inertial/EuRoC.yaml";
SLAM = std::make_unique<ORB_SLAM3::System>(vocab_path.string(), setting_path.string(), ORB_SLAM3::System::IMU_STEREO, false);


#else
setting_path = root_path / "Examples" / "RGB-D" / "ETH3D.yaml";
SLAM = std::make_unique<ORB_SLAM3::System>(vocab_path.string(), setting_path.string(), ORB_SLAM3::System::RGBD, false);
Expand All @@ -59,7 +64,7 @@ class orb_slam3 : public plugin {
virtual void start() override {
plugin::start();
#ifdef STEREO_IMU
sb->schedule<rgb_depth_type>(id, "rgb_depth", [&](switchboard::ptr<const rgb_depth_type> datum, std::size_t iteration_no) {
sb->schedule<imu_type>(id, "imu", [&](switchboard::ptr<const imu_type> datum, std::size_t iteration_no) {
this->feed_imu_cam(datum, iteration_no);
});
#else
Expand All @@ -69,39 +74,33 @@ class orb_slam3 : public plugin {
#endif
}

void feed_imu_cam(switchboard::ptr<const rgb_depth_type> datum, std::size_t iteration_no){
void feed_imu_cam(switchboard::ptr<const imu_type> datum, std::size_t iteration_no){
// Ensures that slam doesnt start before valid IMU readings come in
if (datum == NULL) {
return;
}
if (datum->img0.has_value() || datum->img1.has_value()) {
cam_count++;
}
if (cam_count == 0) {
return;
}

// Get current IMU data
cv::Point3f acc(datum->linear_a.x(), datum->linear_a.y(), datum->linear_a.z());
cv::Point3f gyro(datum->angular_v.x(), datum->angular_v.y(), datum->angular_v.z());
ORB_SLAM3::IMU::Point input_imu(acc.x, acc.y, acc.z, gyro.x, gyro.y, gyro.z, duration2double(datum->time.time_since_epoch()));
current_input.push_back(input_imu);

switchboard::ptr<const cam_type> cam;
// Buffered Async:
cam = _m_cam.size() == 0 ? nullptr : _m_cam.dequeue();
// If there is not cam data this func call, break early
if (!cam) {
return;
}

// If there is cam data, load IMU data from the last cam data up until now
if (datum->img0.has_value() || datum->img1.has_value()) {
if (cam_count > 1) {
prev_input.clear();
for (int i = 0; i < current_input.size(); i++){
ORB_SLAM3::IMU::Point imu_point = current_input[i];
prev_input.push_back(imu_point);
}
current_input.clear();
assert((datum->img1.has_value() && datum->img0.has_value()) || (!datum->img1.has_value() && !datum->img0.has_value()));
}
// If there is not cam data this func call, break early
} else {
return;
prev_input.clear();
for (int i = 0; i < current_input.size(); i++){
ORB_SLAM3::IMU::Point imu_point = current_input[i];
prev_input.push_back(imu_point);
}
current_input.clear();

#ifdef CV_HAS_METRICS
cv::metrics::setAccount(new std::string{std::to_string(iteration_no)});
Expand All @@ -112,13 +111,11 @@ class orb_slam3 : public plugin {
#warning "No OpenCV metrics available. Please recompile OpenCV from git clone --branch 3.4.6-instrumented https://github.com/ILLIXR/opencv/. (see install_deps.sh)"
#endif

cv::Mat cam0{datum->img0.value()};
cv::Mat cam1{datum->img1.value()};
cv::Mat input_cam0 = cam0.clone();
cv::Mat input_cam1 = cam1.clone();
cv::Mat cam0{cam->img0};
cv::Mat cam1{cam->img1};

auto start = std::chrono::steady_clock::now();
Sophus::SE3f mat_pose = SLAM->TrackStereo(input_cam0, input_cam1, duration2double(datum->time.time_since_epoch()), prev_input).inverse();
Sophus::SE3f mat_pose = SLAM->TrackStereo(cam0, cam1, duration2double(cam->time.time_since_epoch()), prev_input).inverse();
auto end = std::chrono::steady_clock::now();

#ifndef NDEBUG
Expand All @@ -128,7 +125,7 @@ class orb_slam3 : public plugin {
min_runtime = std::min(min_runtime, duration);
max_runtime = std::max(max_runtime, duration);
num_runtime++;
printf("timestamp: %f current: %f min: %f max: %f average: %f", duration2double(datum->time.time_since_epoch()), duration,
printf("timestamp: %f current: %f min: %f max: %f average: %f", duration2double(cam->time.time_since_epoch()), duration,
min_runtime, max_runtime, total_runtime / num_runtime);
#endif

Expand All @@ -147,7 +144,7 @@ class orb_slam3 : public plugin {
Eigen::Quaterniond rotd = Eigen::Quaterniond{double(quat.w()),double(quat.x()),double(quat.y()),double(quat.z())};

//dump to file and compare using EVO in both RGBD and Stereo
dumped_pose << std::fixed << std::setprecision(6) << duration2double(datum->time.time_since_epoch()) << setprecision(9)
dumped_pose << std::fixed << std::setprecision(6) << duration2double(cam->time.time_since_epoch()) << setprecision(9)
<< " " << trans[0]
<< " " << trans[1]
<< " " << trans[2]
Expand Down Expand Up @@ -183,13 +180,13 @@ class orb_slam3 : public plugin {
assert(isfinite(rotf.z()));

_m_pose.put(_m_pose.allocate(
datum->time,
cam->time,
trans,
quat
));

_m_imu_integrator_input.put(_m_imu_integrator_input.allocate(
datum->time,
cam->time,
ILLIXR::duration{0L},
imu_params{
SLAM->settings_->noiseGyro(),
Expand Down Expand Up @@ -227,8 +224,8 @@ class orb_slam3 : public plugin {
#endif

// get and clone the images
cv::Mat img{datum->rgb.value()};
cv::Mat depth{datum->depth.value()};
cv::Mat img{datum->rgb};
cv::Mat depth{datum->depth};
cv::Mat input_cam = img.clone();
cv::Mat input_depth = depth.clone();

Expand Down Expand Up @@ -303,6 +300,8 @@ class orb_slam3 : public plugin {
private:
const std::shared_ptr<switchboard> sb;
switchboard::writer<pose_type> _m_pose;
std::shared_ptr<RelativeClock> _m_rtc;
switchboard::buffered_reader<cam_type> _m_cam;
switchboard::writer<imu_integrator_input> _m_imu_integrator_input;
int cam_count;

Expand Down

0 comments on commit d2132c0

Please sign in to comment.