diff --git a/src/vio.cpp b/src/vio.cpp
index c322811..3c3272c 100644
--- a/src/vio.cpp
+++ b/src/vio.cpp
@@ -412,13 +412,13 @@ void DrawCube(const float &size,const float x, const float y, const float z)
 
 void DrawOpenCVCube(cv::Mat& image, const Sophus::SE3d& Trans, const cv::Mat& K_, const std::vector<double>& distCoeffs_){
   cv::Point3d point1 = {xSkew, ySkew, zSkew};
-  cv::Point3d point2 = {xSkew, ySkew+1, zSkew};
-  cv::Point3d point3 = {xSkew+1, ySkew, zSkew};
-  cv::Point3d point4 = {xSkew+1, ySkew+1, zSkew};
-  cv::Point3d point5 = {xSkew, ySkew, zSkew+1};
-  cv::Point3d point6 = {xSkew, ySkew+1, zSkew+1};
-  cv::Point3d point7 = {xSkew+1, ySkew, zSkew+1};
-  cv::Point3d point8 = {xSkew+1, ySkew+1, zSkew+1};
+  cv::Point3d point2 = {xSkew, ySkew+4, zSkew};
+  cv::Point3d point3 = {xSkew+4, ySkew, zSkew};
+  cv::Point3d point4 = {xSkew+4, ySkew+4, zSkew};
+  cv::Point3d point5 = {xSkew, ySkew, zSkew+4};
+  cv::Point3d point6 = {xSkew, ySkew+4, zSkew+4};
+  cv::Point3d point7 = {xSkew+4, ySkew, zSkew+4};
+  cv::Point3d point8 = {xSkew+4, ySkew+4, zSkew+4};
 
   std::vector<cv::Point3d> objectPoints = {point1, point2, point3, point4, point5, point6, point7, point8};
   std::vector<cv::Point2d> imagePoints;
@@ -431,7 +431,7 @@ void DrawOpenCVCube(cv::Mat& image, const Sophus::SE3d& Trans, const cv::Mat& K_
 
   // Projecting points
   cv::projectPoints(objectPoints, rVec, tVec, K_, distCoeffs_, imagePoints);
-  cv::Vec3b color1 = {133, 247, 208};
+  cv::Vec3b color1 = {10, 255, 10};
 
   cv::line(image, imagePoints[0], imagePoints[1], color1, 2);
   cv::line(image, imagePoints[0], imagePoints[2], color1, 2);
@@ -724,21 +724,21 @@ int main(int argc, char** argv) {
     auto cx = intrinsics_vector[2];
     auto cy = intrinsics_vector[3];
 
-//    //    std::cout << "Hessian calib" << std::endl;
-//    K.at<double>(0,0) = 718.856;
-//    //    std::cout << "fx: " << K.at<double>(0, 0);
-//    K.at<double>(1,1) = 718.856;
-//    //    std::cout << "fy: " << K.at<double>(1, 1);
-//    K.at<double>(0,2) = 607.1928;
-//    //    std::cout << "cx: " << K.at<double>(0, 2);
-//    K.at<double>(1,2) = 185.2157;
-//    //    std::cout << "cy: " << K.at<double> (1, 2);
-//
-//    distCoeffs[0] = 0.003482389402;
-//    distCoeffs[1] = 0.000715034845;
-//    distCoeffs[2] = -0.002053236141;
-//    distCoeffs[3] = 0.000202936736;
-//    distCoeffs[4] = 0;
+    //    std::cout << "Hessian calib" << std::endl;
+    K.at<double>(0,0) = 707.092;
+    //    std::cout << "fx: " << K.at<double>(0, 0);
+    K.at<double>(1,1) = 707.0912;
+    //    std::cout << "fy: " << K.at<double>(1, 1);
+    K.at<double>(0,2) = 601.8873;
+    //    std::cout << "cx: " << K.at<double>(0, 2);
+    K.at<double>(1,2) = 183.1104;
+    //    std::cout << "cy: " << K.at<double> (1, 2);
+
+    distCoeffs[0] = 0.003482389402;
+    distCoeffs[1] = 0.000715034845;
+    distCoeffs[2] = -0.002053236141;
+    distCoeffs[3] = 0.000202936736;
+    distCoeffs[4] = 0;
 
     ar_3d_camera = pangolin::OpenGlRenderState(
         pangolin::ProjectionMatrix(1241.0, 376.0, fx, fy, cx, cy, 0.001, 10000),
@@ -839,7 +839,11 @@ int main(int argc, char** argv) {
       } else if (!it_ar->second->frames.empty()) {
         T_w_i = it_ar->second->frames.back();
       }
-
+      auto T_i_c = calib.T_i_c[0];
+      // Draw OpenCV cube
+      if (IfDrawOpenCVCube){
+        DrawOpenCVCube(img_cv_data[2], (T_w_i * T_i_c), K, distCoeffs);
+      }
       // For some reason, there are 4 elements, and two first of them are the empty images.
       if (drawTexture) {
         DrawImageTexture(imageTexture, img_cv_data[2]);
@@ -892,7 +896,7 @@ 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);
@@ -1274,7 +1278,7 @@ void draw_scene_no_camera(pangolin::View& view) {
   if (drawCubeBool){
     DrawCube(cubeSize, xSkew, ySkew, zSkew);
   }
-  else if(drawLineCubeBool){
+  if (drawLineCubeBool){
     drawLinesCube(xSkew, ySkew, zSkew, cubeSize);
   }
   // For the 6th sequence mono-kitti