v4

src/vio.cpp

@@ -71,6 +71,7 @@ OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include "opencv2/imgproc/imgproc.hpp"
 #include "opencv2/calib3d/calib3d.hpp"
 #include "opencv2/highgui/highgui.hpp"
+#include <opencv2/core/eigen.hpp>
 
 using namespace fmt::literals;
 
@@ -340,7 +341,6 @@ void drawLinesCube(float x, float y, float z, int scale){
 void feed_images() {
   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++) {
       if (vio->finished || terminate || (max_frames > 0 && i >= max_frames)) {
         // 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);
     }
-  }
   // Indicate the end of the sequence
   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;
 
   Eigen::Matrix<double, 3, 3> rotation = Trans.so3().matrix();
-  Eigen::Vector3d rvec = rot2rvec(rotation);
-  std::vector<double> rVec(rvec.data(), rvec.data() + rvec.rows() * rvec.cols());
+  cv::Mat mat_rotation;
+  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();
   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);
 
     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);
 
 //    pangolin::View& plot_display = pangolin::CreateDisplay().SetBounds(
@@ -896,7 +901,6 @@ int main(int argc, char** argv) {
 //        LoadCameraPose(Transform_matrix);
 //      }
       // End appended.
-      std::cout << "Number of cameras" << calib.intrinsics.size() << std::endl;
       if (show_frame.GuiChanged()) {
         for (size_t cam_id = 0; cam_id < calib.intrinsics.size(); cam_id++) {
           size_t frame_id = static_cast<size_t>(show_frame);
@@ -1164,12 +1168,20 @@ void draw_scene(pangolin::View& view) {
   UNUSED(view);
   view.Activate(camera);
 
-//  if (drawCubeBool){
-//    DrawCube(cubeSize, xSkew, ySkew, zSkew);
-//  }
-//  else if(drawLineCubeBool){
-//    drawLinesCube(xSkew, ySkew, zSkew, 1);
-//  }
+  if (drawCubeBool){
+    DrawCube(cubeSize, xSkew, ySkew, zSkew);
+  }
+  else if(drawLineCubeBool){
+    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{
 //    view.Activate(T);
 //    throw (0);
@@ -1189,44 +1201,44 @@ void draw_scene(pangolin::View& view) {
   glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
 
   glColor3ubv(cam_color);
-//  if (!vio_t_w_i.empty()) {
-//    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(),
-//                                                  vio_t_w_i.begin() + end);
-//    pangolin::glDrawLineStrip(sub_gt);
-//  }
+  if (!vio_t_w_i.empty()) {
+    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(),
+                                                  vio_t_w_i.begin() + end);
+    pangolin::glDrawLineStrip(sub_gt);
+  }
 
   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;
-//  int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id];
-//  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);
-//  }
+  size_t frame_id = show_frame;
+  int64_t t_ns = vio_dataset->get_image_timestamps()[frame_id];
+  auto it = vis_map.find(t_ns);
 
-//  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) {