Skip to content

Commit

Permalink
Integrated with new Switchboard API (#6)
Browse files Browse the repository at this point in the history 
* Added call to ILLIXR::abort (#7)

* Testing integration with GTSAM v4.0.3 and switchboard

* Fixing integration with c++17

* Fixed mismatch alloc-deallocs reported as undefined behavior, and added updated .gitignore

* Updating switchboard integration to use fowarded constructor args allocation (#5)

* Added missing gtsam header
  • Loading branch information
@e3m3
e3m3 authored May 12, 2021
1 parent d8f47ed commit 4bf387d
Show file tree
Hide file tree
Showing 5 changed files with 166 additions and 148 deletions.
4 changes: 4 additions & 0 deletions .gitignore
View file
Original file line number Diff line number Diff line change
Expand Up @@ -29,3 +29,7 @@ common

% Ignore large vocabulary files
/vocabulary/*

% Ignore objects
*.dbg.so
*.opt.so
299 changes: 157 additions & 142 deletions examples/plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,190 +13,205 @@
#include "../common/switchboard.hpp"
#include "../common/data_format.hpp"
#include "../common/phonebook.hpp"
#include "../common/error_util.hpp"

using namespace ILLIXR;

std::string get_path() {
const char* KIMERA_ROOT_c_str = std::getenv("KIMERA_ROOT");
if (!KIMERA_ROOT_c_str) {
std::cerr << "Please define KIMERA_ROOT" << std::endl;
abort();
ILLIXR::abort("Please define KIMERA_ROOT");
}
std::string KIMERA_ROOT = std::string{KIMERA_ROOT_c_str};
return KIMERA_ROOT + "params/ILLIXR";
}

class kimera_vio : public plugin {
public:
/* Provide handles to kimera_vio */
kimera_vio(std::string name_, phonebook* pb_)
: plugin{name_, pb_}
, sb{pb->lookup_impl<switchboard>()}
, kimera_current_frame_id(0)
, kimera_pipeline_params(get_path())
, kimera_pipeline(kimera_pipeline_params)
, _m_pose{sb->publish<pose_type>("slow_pose")}
, _m_imu_integrator_input{sb->publish<imu_integrator_input>("imu_integrator_input")}
{
_m_begin = std::chrono::system_clock::now();
imu_cam_buffer = NULL;

// TODO: read Kimera flag file path from runner and find a better way of passing it to gflag
kimera_pipeline.registerBackendOutputCallback(
std::bind(
&kimera_vio::pose_callback,
this,
std::placeholders::_1
)
);

_m_pose->put(
new pose_type{
.sensor_time = std::chrono::time_point<std::chrono::system_clock>{},
.position = Eigen::Vector3f{0, 0, 0},
.orientation = Eigen::Quaternionf{1, 0, 0, 0}
}
);
/* Provide handles to kimera_vio */
kimera_vio(std::string name_, phonebook* pb_)
: plugin{name_, pb_}
, sb{pb->lookup_impl<switchboard>()}
, kimera_current_frame_id(0)
, kimera_pipeline_params(get_path())
, kimera_pipeline(kimera_pipeline_params)
, _m_pose{sb->get_writer<pose_type>("slow_pose")}
, _m_imu_integrator_input{sb->get_writer<imu_integrator_input>("imu_integrator_input")}
, imu_cam_buffer{nullptr}
{
_m_begin = std::chrono::system_clock::now();
// TODO: read Kimera flag file path from runner and find a better way of passing it to gflag
kimera_pipeline.registerBackendOutputCallback(
std::bind(
&kimera_vio::pose_callback,
this,
std::placeholders::_1
)
);

pose_type datum_pose_tmp{
ILLIXR::time_type{},
Eigen::Vector3f{0, 0, 0},
Eigen::Quaternionf{1, 0, 0, 0}
};
switchboard::ptr<pose_type> datum_pose = _m_pose.allocate<pose_type>(std::move(datum_pose_tmp));
_m_pose.put(std::move(datum_pose));

#ifdef CV_HAS_METRICS
cv::metrics::setAccount(new std::string{"-1"});
cv::metrics::setAccount(new std::string{"-1"});
#endif
}

}

virtual void start() override {
plugin::start();
sb->schedule<imu_cam_type>(id, "imu_cam", [&](const imu_cam_type *datum) {
this->feed_imu_cam(datum);
});

}

virtual void start() override {
plugin::start();
sb->schedule<imu_cam_type>(id, "imu_cam", [this](switchboard::ptr<const imu_cam_type> datum, std::size_t) {
this->feed_imu_cam(datum);
});
}

std::size_t iteration_no = 0;
void feed_imu_cam(const imu_cam_type *datum) {

// Ensures that slam doesnt start before valid IMU readings come in
if (datum == NULL) {
assert(previous_timestamp == 0);
return;
}
std::size_t iteration_no = 0;
void feed_imu_cam(switchboard::ptr<const imu_cam_type> datum) {
// Ensures that slam doesnt start before valid IMU readings come in
if (datum == nullptr) {
assert(previous_timestamp == 0);
return;
}

// This ensures that every data point is coming in chronological order If youre failing this assert,
// make sure that your data folder matches the name in offline_imu_cam/plugin.cc
assert(datum->dataset_time > previous_timestamp);
previous_timestamp = datum->dataset_time;
// This ensures that every data point is coming in chronological order If youre failing this assert,
// make sure that your data folder matches the name in offline_imu_cam/plugin.cc
assert(datum->dataset_time > previous_timestamp);
previous_timestamp = datum->dataset_time;

imu_cam_buffer = datum;
// Store the datum back to our buffer
imu_cam_buffer = datum;

VIO::Vector6 imu_raw_vals;
imu_raw_vals << datum->linear_a.cast<double>(), datum->angular_v.cast<double>();
VIO::Vector6 imu_raw_vals;
imu_raw_vals << datum->linear_a.cast<double>(), datum->angular_v.cast<double>();

// Feed the IMU measurement. There should always be IMU data in each call to feed_imu_cam
assert((datum->img0.has_value() && datum->img1.has_value()) || (!datum->img0.has_value() && !datum->img1.has_value()));
kimera_pipeline.fillSingleImuQueue(VIO::ImuMeasurement(datum->dataset_time, imu_raw_vals));
// Feed the IMU measurement. There should always be IMU data in each call to feed_imu_cam
assert((datum->img0.has_value() && datum->img1.has_value()) || (!datum->img0.has_value() && !datum->img1.has_value()));
kimera_pipeline.fillSingleImuQueue(VIO::ImuMeasurement(datum->dataset_time, imu_raw_vals));

// If there is not cam data this func call, break early
if (!datum->img0.has_value() && !datum->img1.has_value()) {
return;
}
// If there is not cam data this func call, break early
if (!datum->img0.has_value() && !datum->img1.has_value()) {
return;
}

#ifdef CV_HAS_METRICS
cv::metrics::setAccount(new std::string{std::to_string(iteration_no)});
iteration_no++;
if (iteration_no % 20 == 0) {
cv::metrics::dump();
}
cv::metrics::setAccount(new std::string{std::to_string(iteration_no)});
iteration_no++;
if (iteration_no % 20 == 0) {
cv::metrics::dump();
}
#else
#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 img0{*imu_cam_buffer->img0.value()};
cv::Mat img1{*imu_cam_buffer->img1.value()};
cv::Mat img0{imu_cam_buffer->img0.value()};
cv::Mat img1{imu_cam_buffer->img1.value()};

// VIOParams
VIO::CameraParams left_cam_info = kimera_pipeline_params.camera_params_.at(0);
VIO::CameraParams right_cam_info = kimera_pipeline_params.camera_params_.at(1);
kimera_pipeline.fillLeftFrameQueue(VIO::make_unique<VIO::Frame>(kimera_current_frame_id,
datum->dataset_time,
left_cam_info, img0));
kimera_pipeline.fillRightFrameQueue(VIO::make_unique<VIO::Frame>(kimera_current_frame_id,
datum->dataset_time,
right_cam_info, img1));
// VIOParams
VIO::CameraParams left_cam_info = kimera_pipeline_params.camera_params_.at(0);
VIO::CameraParams right_cam_info = kimera_pipeline_params.camera_params_.at(1);
kimera_pipeline.fillLeftFrameQueue(VIO::make_unique<VIO::Frame>(kimera_current_frame_id,
datum->dataset_time,
left_cam_info, img0));
kimera_pipeline.fillRightFrameQueue(VIO::make_unique<VIO::Frame>(kimera_current_frame_id,
datum->dataset_time,
right_cam_info, img1));

// Ok this needs to be filed in a later task to dig up what sets errno in Kimera
assert(errno == 0);
kimera_pipeline.spin();
errno = 0;

#ifndef NDEBUG
std::cout << "SPIN FULL\n";
std::cout << "SPIN FULL\n";
#endif
}

void pose_callback(const std::shared_ptr<VIO::BackendOutput>& vio_output) {

const auto& cached_state = vio_output->W_State_Blkf_;
const auto& w_pose_blkf_trans = cached_state.pose_.translation().transpose();
const auto& w_pose_blkf_rot = cached_state.pose_.rotation().quaternion();
const auto& w_vel_blkf = cached_state.velocity_.transpose();
const auto& imu_bias_gyro = cached_state.imu_bias_.gyroscope().transpose();
const auto& imu_bias_acc = cached_state.imu_bias_.accelerometer().transpose();
// Get the pose returned from SLAM
Eigen::Quaternionf quat = Eigen::Quaternionf{w_pose_blkf_rot(0), w_pose_blkf_rot(1), w_pose_blkf_rot(2), w_pose_blkf_rot(3)};
Eigen::Quaterniond doub_quat = Eigen::Quaterniond{w_pose_blkf_rot(0), w_pose_blkf_rot(1), w_pose_blkf_rot(2), w_pose_blkf_rot(3)};
Eigen::Vector3f pos = w_pose_blkf_trans.cast<float>();

assert(isfinite(quat.w()));
assert(isfinite(quat.x()));
assert(isfinite(quat.y()));
assert(isfinite(quat.z()));
assert(isfinite(pos[0]));
assert(isfinite(pos[1]));
assert(isfinite(pos[2]));

_m_pose->put(new pose_type{
.sensor_time = imu_cam_buffer->time,
.position = pos,
.orientation = quat,
});

_m_imu_integrator_input->put(new imu_integrator_input{
.last_cam_integration_time = (double(imu_cam_buffer->dataset_time) / NANO_SEC),
.t_offset = -0.05,

.params = {
.gyro_noise = kimera_pipeline_params.imu_params_.gyro_noise_,
.acc_noise = kimera_pipeline_params.imu_params_.acc_noise_,
.gyro_walk = kimera_pipeline_params.imu_params_.gyro_walk_,
.acc_walk = kimera_pipeline_params.imu_params_.acc_walk_,
.n_gravity = kimera_pipeline_params.imu_params_.n_gravity_,
.imu_integration_sigma = kimera_pipeline_params.imu_params_.imu_integration_sigma_,
.nominal_rate = kimera_pipeline_params.imu_params_.nominal_rate_,
},

.biasAcc =imu_bias_acc,
.biasGyro = imu_bias_gyro,
.position = w_pose_blkf_trans,
.velocity = w_vel_blkf,
.quat = doub_quat,
});
}

virtual ~kimera_vio() override {}
}

void pose_callback(const std::shared_ptr<VIO::BackendOutput>& vio_output) {

const auto& cached_state = vio_output->W_State_Blkf_;
const auto& w_pose_blkf_trans = cached_state.pose_.translation().transpose();
const auto& w_pose_blkf_rot = cached_state.pose_.rotation().quaternion();
const auto& w_vel_blkf = cached_state.velocity_.transpose();
const auto& imu_bias_gyro = cached_state.imu_bias_.gyroscope().transpose();
const auto& imu_bias_acc = cached_state.imu_bias_.accelerometer().transpose();
// Get the pose returned from SLAM
Eigen::Quaternionf quat = Eigen::Quaternionf{
static_cast<float>(w_pose_blkf_rot(0)),
static_cast<float>(w_pose_blkf_rot(1)),
static_cast<float>(w_pose_blkf_rot(2)),
static_cast<float>(w_pose_blkf_rot(3))
};
Eigen::Quaterniond doub_quat = Eigen::Quaterniond{
w_pose_blkf_rot(0),
w_pose_blkf_rot(1),
w_pose_blkf_rot(2),
w_pose_blkf_rot(3)
};
Eigen::Vector3f pos = w_pose_blkf_trans.cast<float>();

assert(isfinite(quat.w()));
assert(isfinite(quat.x()));
assert(isfinite(quat.y()));
assert(isfinite(quat.z()));
assert(isfinite(pos[0]));
assert(isfinite(pos[1]));
assert(isfinite(pos[2]));

pose_type datum_pose_tmp{
imu_cam_buffer->time,
pos,
quat,
};
switchboard::ptr<pose_type> datum_pose = _m_pose.allocate<pose_type>(std::move(datum_pose_tmp));
_m_pose.put(std::move(datum_pose));

imu_integrator_input datum_imu_int_tmp{
(static_cast<double>(imu_cam_buffer->dataset_time) / NANO_SEC),
-0.05,

{
kimera_pipeline_params.imu_params_.gyro_noise_,
kimera_pipeline_params.imu_params_.acc_noise_,
kimera_pipeline_params.imu_params_.gyro_walk_,
kimera_pipeline_params.imu_params_.acc_walk_,
kimera_pipeline_params.imu_params_.n_gravity_,
kimera_pipeline_params.imu_params_.imu_integration_sigma_,
kimera_pipeline_params.imu_params_.nominal_rate_,
},

imu_bias_acc,
imu_bias_gyro,
w_pose_blkf_trans,
w_vel_blkf,
doub_quat,
};
switchboard::ptr<imu_integrator_input> datum_imu_int =
_m_imu_integrator_input.allocate<imu_integrator_input>(std::move(datum_imu_int_tmp));
_m_imu_integrator_input.put(std::move(datum_imu_int));
}

virtual ~kimera_vio() override {}

private:
const std::shared_ptr<switchboard> sb;
std::unique_ptr<writer<pose_type>> _m_pose;
std::unique_ptr<writer<imu_integrator_input>> _m_imu_integrator_input;
time_type _m_begin;

VIO::FrameId kimera_current_frame_id;
VIO::VioParams kimera_pipeline_params;
VIO::Pipeline kimera_pipeline;

const imu_cam_type* imu_cam_buffer;
double previous_timestamp = 0.0;
const std::shared_ptr<switchboard> sb;
switchboard::writer<pose_type> _m_pose;
switchboard::writer<imu_integrator_input> _m_imu_integrator_input;
time_type _m_begin;

VIO::FrameId kimera_current_frame_id;
VIO::VioParams kimera_pipeline_params;
VIO::Pipeline kimera_pipeline;

//const imu_cam_type* imu_cam_buffer;
switchboard::ptr<const imu_cam_type> imu_cam_buffer;
double previous_timestamp = 0.0;
};

PLUGIN_MAIN(kimera_vio)
1 change: 1 addition & 0 deletions include/kimera-vio/backend/VioBackEndParams.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@

#include <gtsam/base/Vector.h>
#include <gtsam/slam/SmartFactorParams.h>
#include <gtsam/linear/HessianFactor.h>

#include <glog/logging.h>

Expand Down
6 changes: 2 additions & 4 deletions include/kimera-vio/frontend/feature-detector/anms/range-tree/ranget.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -468,8 +468,7 @@ template <class point, class data> class rangetree {
nuke(start.right);

if (!isptypooled(start.ypoints))
delete start.ypoints;
//free(start.ypoints);
free(start.ypoints);
}

void nuke(node * const n) {
Expand All @@ -480,8 +479,7 @@ template <class point, class data> class rangetree {
nuke(n->right);

if (!isptypooled(n->ypoints))
delete n->ypoints;
//free(n->ypoints);
free(n->ypoints);


if (!isnodepooled(n))
Expand Down
Loading

0 comments on commit 4bf387d

Please sign in to comment.