pi/ar_basalt
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

v4

Browse Source
This commit is contained in:
Ivan
2022-04-14 15:55:59 +03:00
parent 34d7abd905
commit 2a22c4ef7c
1 changed files with 58 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

104
src/vio.cpp
View File

@@ -71,6 +71,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "opencv2/imgproc/imgproc.hpp" #include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/calib3d/calib3d.hpp" #include "opencv2/calib3d/calib3d.hpp"
#include "opencv2/highgui/highgui.hpp" #include "opencv2/highgui/highgui.hpp"
#include <opencv2/core/eigen.hpp>
using namespace fmt::literals; using namespace fmt::literals;
@@ -340,7 +341,6 @@ void drawLinesCube(float x, float y, float z, int scale){
void feed_images() { void feed_images() {
std::cout << "Started input_data thread " << std::endl; std::cout << "Started input_data thread " << std::endl;
for (int iterator=0; iterator<2;iterator++){
for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) { for (size_t i = 0; i < vio_dataset->get_image_timestamps().size(); i++) {
if (vio->finished || terminate || (max_frames > 0 && i >= max_frames)) { if (vio->finished || terminate || (max_frames > 0 && i >= max_frames)) {
// stop loop early if we set a limit on number of frames to process // stop loop early if we set a limit on number of frames to process
@@ -362,7 +362,6 @@ void feed_images() {
opt_flow_ptr->input_queue.push(data); opt_flow_ptr->input_queue.push(data);
} }
}
// Indicate the end of the sequence // Indicate the end of the sequence
opt_flow_ptr->input_queue.push(nullptr); opt_flow_ptr->input_queue.push(nullptr);
@@ -424,8 +423,14 @@ void DrawOpenCVCube(cv::Mat& image, const Sophus::SE3d& Trans, const cv::Mat& K_
std::vector<cv::Point2d> imagePoints; std::vector<cv::Point2d> imagePoints;
Eigen::Matrix<double, 3, 3> rotation = Trans.so3().matrix(); Eigen::Matrix<double, 3, 3> rotation = Trans.so3().matrix();
Eigen::Vector3d rvec = rot2rvec(rotation); cv::Mat mat_rotation;
std::vector<double> rVec(rvec.data(), rvec.data() + rvec.rows() * rvec.cols()); cv::eigen2cv(rotation, mat_rotation);
// Eigen::Vector3d rvec = rot2rvec(rotation);
cv::Mat rvec;
cv::Rodrigues(mat_rotation, rvec);
// std::vector<double> rVec(rvec.data(), rvec.data() + rvec.rows() * rvec.cols());
std::vector<double> rVec = rvec.clone();
std::cout << "rVec size: " << " " << rVec.size() << std::endl;
Eigen::Vector3d tvec = Trans.translation(); Eigen::Vector3d tvec = Trans.translation();
std::vector<double> tVec(tvec.data(), tvec.data() + tvec.rows() * tvec.cols()); std::vector<double> tVec(tvec.data(), tvec.data() + tvec.rows() * tvec.cols());
@@ -674,7 +679,7 @@ int main(int argc, char** argv) {
0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0); 0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0);
pangolin::View& img_view_display = pangolin::CreateDisplay() pangolin::View& img_view_display = pangolin::CreateDisplay()
.SetBounds(0.0, 1.0, pangolin::Attach::Pix(UI_WIDTH), 1.0) .SetBounds(0.4, 1.0, 0, 0.4)
.SetLayout(pangolin::LayoutEqual); .SetLayout(pangolin::LayoutEqual);
// pangolin::View& plot_display = pangolin::CreateDisplay().SetBounds( // pangolin::View& plot_display = pangolin::CreateDisplay().SetBounds(
@@ -896,7 +901,6 @@ int main(int argc, char** argv) {
// LoadCameraPose(Transform_matrix); // LoadCameraPose(Transform_matrix);
// } // }
// End appended. // End appended.
std::cout << "Number of cameras" << calib.intrinsics.size() << std::endl;
if (show_frame.GuiChanged()) { if (show_frame.GuiChanged()) {
for (size_t cam_id = 0; cam_id < calib.intrinsics.size(); cam_id++) { for (size_t cam_id = 0; cam_id < calib.intrinsics.size(); cam_id++) {
size_t frame_id = static_cast<size_t>(show_frame); size_t frame_id = static_cast<size_t>(show_frame);
@@ -1164,12 +1168,20 @@ void draw_scene(pangolin::View& view) {
UNUSED(view); UNUSED(view);
view.Activate(camera); view.Activate(camera);
// if (drawCubeBool){ if (drawCubeBool){
// DrawCube(cubeSize, xSkew, ySkew, zSkew); DrawCube(cubeSize, xSkew, ySkew, zSkew);
// } }
// else if(drawLineCubeBool){ else if(drawLineCubeBool){
// drawLinesCube(xSkew, ySkew, zSkew, 1); drawLinesCube(xSkew, ySkew, zSkew, 1);
// } }
// For the 6th sequence mono-kitti
if (showCube1) {drawLinesCube(-5.5, 0.5, -35.0, cubeSize);}
if (showCube2) {drawLinesCube(5.5, 0.5, -40.0, cubeSize);}
if (showCube3) {drawLinesCube(-5.5, 1.0, -95.0, cubeSize);}
if (showCube4) {drawLinesCube(5.5, 1.0, -150.0, cubeSize);}
if (showCube5) {drawLinesCube(16.0, 4.8, -324.5, cubeSize);}
if (showCube6) {drawLinesCube(32.8, 4.8, -324.5, cubeSize);}
if (showCube7) {drawLinesCube(32.5, 4.8, -345.7, cubeSize);}
// try{ // try{
// view.Activate(T); // view.Activate(T);
// throw (0); // throw (0);
@@ -1189,44 +1201,44 @@ void draw_scene(pangolin::View& view) {
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA); glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glColor3ubv(cam_color); glColor3ubv(cam_color);
// if (!vio_t_w_i.empty()) { if (!vio_t_w_i.empty()) {
// size_t end = std::min(vio_t_w_i.size(), size_t(show_frame + 1)); size_t end = std::min(vio_t_w_i.size(), size_t(show_frame + 1));
// Eigen::aligned_vector<Eigen::Vector3d> sub_gt(vio_t_w_i.begin(), Eigen::aligned_vector<Eigen::Vector3d> sub_gt(vio_t_w_i.begin(),
// vio_t_w_i.begin() + end); vio_t_w_i.begin() + end);
// pangolin::glDrawLineStrip(sub_gt); pangolin::glDrawLineStrip(sub_gt);
// } }
glColor3ubv(gt_color); glColor3ubv(gt_color);
// if (show_gt) pangolin::glDrawLineStrip(gt_t_w_i); if (show_gt) pangolin::glDrawLineStrip(gt_t_w_i);
// size_t frame_id = show_frame; size_t frame_id = show_frame;
// int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id]; int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id];
// auto it = vis_map.find(t_ns); auto it = vis_map.find(t_ns);
//
// if (it != vis_map.end()) {
// // T_i_c - transformation from camera coordinate point to the imu coordinate point.
// for (size_t i = 0; i < calib.T_i_c.size(); i++)
// if (!it->second->states.empty()) {
// render_camera((it->second->states.back() * calib.T_i_c[i]).matrix(),
// 2.0f, cam_color, 0.1f);
// } else if (!it->second->frames.empty()) {
// render_camera((it->second->frames.back() * calib.T_i_c[i]).matrix(),
// 2.0f, cam_color, 0.1f);
// }
//
// for (const auto& p : it->second->states)
// for (size_t i = 0; i < calib.T_i_c.size(); i++)
// render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, state_color, 0.1f);
//
// for (const auto& p : it->second->frames)
// for (size_t i = 0; i < calib.T_i_c.size(); i++)
// render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, pose_color, 0.1f);
//
// glColor3ubv(pose_color);
// pangolin::glDrawPoints(it->second->points);
// }
// pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0); if (it != vis_map.end()) {
// T_i_c - transformation from camera coordinate point to the imu coordinate point.
for (size_t i = 0; i < calib.T_i_c.size(); i++)
if (!it->second->states.empty()) {
render_camera((it->second->states.back() * calib.T_i_c[i]).matrix(),
2.0f, cam_color, 0.1f);
} else if (!it->second->frames.empty()) {
render_camera((it->second->frames.back() * calib.T_i_c[i]).matrix(),
2.0f, cam_color, 0.1f);
}
for (const auto& p : it->second->states)
for (size_t i = 0; i < calib.T_i_c.size(); i++)
render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, state_color, 0.1f);
for (const auto& p : it->second->frames)
for (size_t i = 0; i < calib.T_i_c.size(); i++)
render_camera((p * calib.T_i_c[i]).matrix(), 2.0f, pose_color, 0.1f);
glColor3ubv(pose_color);
pangolin::glDrawPoints(it->second->points);
}
pangolin::glDrawAxis(Sophus::SE3d().matrix(), 1.0);
} }
void draw_scene_no_camera(pangolin::View& view) { void draw_scene_no_camera(pangolin::View& view) {