ktyl
/
basalt
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

small fixes after merge

This commit is contained in:
Vladyslav Usenko 2019-06-13 13:51:10 +02:00
parent 53287c5588
commit 6cd8058a33
3 changed files with 9 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/basalt/device/rs_t265.h
View File

@ -47,8 +47,6 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pangolin/image/typed_image.h> #include <pangolin/image/typed_image.h>
#include <pangolin/pangolin.h> #include <pangolin/pangolin.h>
#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <tbb/concurrent_queue.h> #include <tbb/concurrent_queue.h>

15
src/rs_t265_vio.cpp
View File

@ -178,12 +178,12 @@ int main(int argc, char** argv) {
if (show_gui) vio->out_vis_queue = &out_vis_queue; if (show_gui) vio->out_vis_queue = &out_vis_queue;
vio->out_state_queue = &out_state_queue; vio->out_state_queue = &out_state_queue;
// basalt::MargDataSaver::Ptr marg_data_saver; basalt::MargDataSaver::Ptr marg_data_saver;
//
// if (!marg_data_path.empty()) { if (!marg_data_path.empty()) {
// marg_data_saver.reset(new basalt::MargDataSaver(marg_data_path)); marg_data_saver.reset(new basalt::MargDataSaver(marg_data_path));
// vio->out_marg_queue = &marg_data_saver->in_marg_queue; vio->out_marg_queue = &marg_data_saver->in_marg_queue;
// } }
vio_data_log.Clear(); vio_data_log.Clear();
@ -282,8 +282,9 @@ int main(int argc, char** argv) {
std::bind(&draw_image_overlay, std::placeholders::_1, idx); std::bind(&draw_image_overlay, std::placeholders::_1, idx);
} }
Eigen::Vector3d cam_p(-0.5, -3, -5); Eigen::Vector3d cam_p(0.5, -2, -2);
cam_p = vio->getT_w_i_init().so3() * calib.T_i_c[0].so3() * cam_p; cam_p = vio->getT_w_i_init().so3() * calib.T_i_c[0].so3() * cam_p;
cam_p[2] = 1;
pangolin::OpenGlRenderState camera( pangolin::OpenGlRenderState camera(
pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000), pangolin::ProjectionMatrix(640, 480, 400, 400, 320, 240, 0.001, 10000),

1
src/vi_estimator/keypoint_vio.cpp
View File

@ -94,6 +94,7 @@ void KeypointVioEstimator::initialize(int64_t t_ns, const Sophus::SE3d& T_w_i,
initialized = true; initialized = true;
T_w_i_init = T_w_i; T_w_i_init = T_w_i;
last_state_t_ns = t_ns;
imu_meas[t_ns] = IntegratedImuMeasurement(t_ns, bg, ba); imu_meas[t_ns] = IntegratedImuMeasurement(t_ns, bg, ba);
frame_states[t_ns] = frame_states[t_ns] =
PoseVelBiasStateWithLin(t_ns, T_w_i, vel_w_i, bg, ba, true); PoseVelBiasStateWithLin(t_ns, T_w_i, vel_w_i, bg, ba, true);