Added - Support for downsizing the image in half (also fix display fo…

…r small resolution images looking bad)
rpng · Apr 25, 2020 · 18bd54f · 18bd54f
1 parent b66e309
commit 18bd54f
Show file tree

Hide file tree

Showing 16 changed files with 126 additions and 67 deletions.
diff --git a/ov_core/package.xml b/ov_core/package.xml
@@ -11,7 +11,6 @@
     <author email="[email protected]">Patrick Geneva</author>
     <author email="[email protected]">Kevin Eckenhoff</author>
     <maintainer email="[email protected]">Patrick Geneva</maintainer>
-    <maintainer email="[email protected]">Kevin Eckenhoff</maintainer>
 
     <!-- Licensing -->
     <license>GNU General Public License v3.0</license>

diff --git a/ov_core/src/track/TrackAruco.cpp b/ov_core/src/track/TrackAruco.cpp
@@ -262,6 +262,9 @@ void TrackAruco::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2
         if(max_height < pair.second.rows) max_height = pair.second.rows;
     }
 
+    // If the image is "small" thus we shoudl use smaller display codes
+    bool is_small = (std::min(max_width,max_height) < 400);
+
     // If the image is "new" then draw the images from scratch
     // Otherwise, we grab the subset of the main image and draw on top of it
     bool image_new = ((int)img_last_cache.size()*max_width != img_out.cols || max_height != img_out.rows);
@@ -282,7 +285,8 @@ void TrackAruco::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2
         if(!ids_aruco[pair.first].empty()) cv::aruco::drawDetectedMarkers(img_temp, corners[pair.first], ids_aruco[pair.first]);
         if(!rejects[pair.first].empty()) cv::aruco::drawDetectedMarkers(img_temp, rejects[pair.first], cv::noArray(), cv::Scalar(100,0,255));
         // Draw what camera this is
-        cv::putText(img_temp, "CAM:"+std::to_string((int)pair.first), cv::Point(30,60), cv::FONT_HERSHEY_COMPLEX_SMALL, 3.0, cv::Scalar(0,255,0),3);
+        auto txtpt = (is_small)? cv::Point(10,30) : cv::Point(30,60);
+        cv::putText(img_temp, "CAM:"+std::to_string((int)pair.first), txtpt, cv::FONT_HERSHEY_COMPLEX_SMALL, (is_small) ? 1.5 : 3.0, cv::Scalar(0,255,0), 3);
         // Replace the output image
         img_temp.copyTo(img_out(cv::Rect(max_width*index_cam,0,img_last_cache[pair.first].cols,img_last_cache[pair.first].rows)));
         index_cam++;

diff --git a/ov_core/src/track/TrackBase.cpp b/ov_core/src/track/TrackBase.cpp
@@ -44,6 +44,9 @@ void TrackBase::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2,
     if(max_width==-1 || max_height==-1)
         return;
 
+    // If the image is "small" thus we shoudl use smaller display codes
+    bool is_small = (std::min(max_width,max_height) < 400);
+
     // If the image is "new" then draw the images from scratch
     // Otherwise, we grab the subset of the main image and draw on top of it
     bool image_new = ((int)img_last_cache.size()*max_width != img_out.cols || max_height != img_out.rows);
@@ -65,15 +68,16 @@ void TrackBase::display_active(cv::Mat &img_out, int r1, int g1, int b1, int r2,
             // Get bounding pts for our boxes
             cv::Point2f pt_l = pts_last[pair.first].at(i).pt;
             // Draw the extracted points and ID
-            cv::circle(img_temp, pt_l, 2, cv::Scalar(r1,g1,b1), CV_FILLED);
+            cv::circle(img_temp, pt_l, (is_small)? 1 : 2, cv::Scalar(r1,g1,b1), CV_FILLED);
             //cv::putText(img_out, std::to_string(ids_left_last.at(i)), pt_l, cv::FONT_HERSHEY_SIMPLEX,0.5,cv::Scalar(0,0,255),1,cv::LINE_AA);
             // Draw rectangle around the point
             cv::Point2f pt_l_top = cv::Point2f(pt_l.x-5,pt_l.y-5);
             cv::Point2f pt_l_bot = cv::Point2f(pt_l.x+5,pt_l.y+5);
             cv::rectangle(img_temp,pt_l_top,pt_l_bot, cv::Scalar(r2,g2,b2), 1);
         }
         // Draw what camera this is
-        cv::putText(img_temp, "CAM:"+std::to_string((int)pair.first), cv::Point(30,60), cv::FONT_HERSHEY_COMPLEX_SMALL, 3.0, cv::Scalar(0,255,0),3);
+        auto txtpt = (is_small)? cv::Point(10,30) : cv::Point(30,60);
+        cv::putText(img_temp, "CAM:"+std::to_string((int)pair.first), txtpt, cv::FONT_HERSHEY_COMPLEX_SMALL, (is_small)? 1.5 : 3.0, cv::Scalar(0,255,0), 3);
         // Replace the output image
         img_temp.copyTo(img_out(cv::Rect(max_width*index_cam,0,img_last_cache[pair.first].cols,img_last_cache[pair.first].rows)));
         index_cam++;
@@ -102,6 +106,9 @@ void TrackBase::display_history(cv::Mat &img_out, int r1, int g1, int b1, int r2
     if(max_width==-1 || max_height==-1)
         return;
 
+    // If the image is "small" thus we shoudl use smaller display codes
+    bool is_small = (std::min(max_width,max_height) < 400);
+
     // If the image is "new" then draw the images from scratch
     // Otherwise, we grab the subset of the main image and draw on top of it
     bool image_new = ((int)img_last_cache.size()*max_width != img_out.cols || max_height != img_out.rows);
@@ -141,7 +148,7 @@ void TrackBase::display_history(cv::Mat &img_out, int r1, int g1, int b1, int r2
                 int color_b = (is_stereo? g2 : b2)-(int)((is_stereo? g1 : b1)/feat->uvs[pair.first].size()*z);
                 // Draw current point
                 cv::Point2f pt_c(feat->uvs[pair.first].at(z)(0),feat->uvs[pair.first].at(z)(1));
-                cv::circle(img_temp, pt_c, 2, cv::Scalar(color_r,color_g,color_b), CV_FILLED);
+                cv::circle(img_temp, pt_c, (is_small)? 1 : 2, cv::Scalar(color_r,color_g,color_b), CV_FILLED);
                 // If there is a next point, then display the line from this point to the next
                 if(z+1 < feat->uvs[pair.first].size()) {
                     cv::Point2f pt_n(feat->uvs[pair.first].at(z+1)(0),feat->uvs[pair.first].at(z+1)(1));
@@ -155,7 +162,8 @@ void TrackBase::display_history(cv::Mat &img_out, int r1, int g1, int b1, int r2
             }
         }
         // Draw what camera this is
-        cv::putText(img_temp, "CAM:"+std::to_string((int)pair.first), cv::Point(30,60), cv::FONT_HERSHEY_COMPLEX_SMALL, 3.0, cv::Scalar(0,255,0),3);
+        auto txtpt = (is_small)? cv::Point(10,30) : cv::Point(30,60);
+        cv::putText(img_temp, "CAM:"+std::to_string((int)pair.first), txtpt, cv::FONT_HERSHEY_COMPLEX_SMALL, (is_small)? 1.5 : 3.0, cv::Scalar(0,255,0), 3);
         // Replace the output image
         img_temp.copyTo(img_out(cv::Rect(max_width*index_cam,0,img_last_cache[pair.first].cols,img_last_cache[pair.first].rows)));
         index_cam++;

diff --git a/ov_core/src/track/TrackKLT.cpp b/ov_core/src/track/TrackKLT.cpp
@@ -235,7 +235,7 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
     // Loop through all left points
     for(size_t i=0; i<pts_left_new.size(); i++) {
         // Ensure we do not have any bad KLT tracks (i.e., points are negative)
-        if(pts_left_new[i].pt.x < 0 || pts_left_new[i].pt.y < 0)
+        if(pts_left_new[i].pt.x < 0 || pts_left_new[i].pt.y < 0 || (int)pts_right_new[i].pt.x > img_left.cols || (int)pts_right_new[i].pt.y > img_left.rows)
             continue;
         // See if we have the same feature in the right
         bool found_right = false;
@@ -251,7 +251,7 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
         // Else track it as a mono feature in just the left image
         if(mask_ll[i] && found_right && mask_rr[index_right]) {
             // Ensure we do not have any bad KLT tracks (i.e., points are negative)
-            if(pts_right_new.at(index_right).pt.x < 0 || pts_right_new.at(index_right).pt.y < 0)
+            if(pts_right_new.at(index_right).pt.x < 0 || pts_right_new.at(index_right).pt.y < 0 || (int)pts_right_new[i].pt.x > img_right.cols || (int)pts_right_new[i].pt.y > img_right.rows)
                 continue;
             good_left.push_back(pts_left_new.at(i));
             good_right.push_back(pts_right_new.at(index_right));
@@ -268,7 +268,7 @@ void TrackKLT::feed_stereo(double timestamp, cv::Mat &img_leftin, cv::Mat &img_r
     // Loop through all right points
     for(size_t i=0; i<pts_right_new.size(); i++) {
         // Ensure we do not have any bad KLT tracks (i.e., points are negative)
-        if(pts_right_new[i].pt.x < 0 || pts_right_new[i].pt.y < 0)
+        if(pts_right_new[i].pt.x < 0 || pts_right_new[i].pt.y < 0 || (int)pts_right_new[i].pt.x > img_right.cols || (int)pts_right_new[i].pt.y > img_right.rows)
             continue;
         // See if we have the same feature in the right
         bool added_already = (std::find(good_ids_right.begin(),good_ids_right.end(),ids_last[cam_id_right].at(i))!=good_ids_right.end());
@@ -323,7 +323,9 @@ void TrackKLT::perform_detection_monocular(const std::vector<cv::Mat> &img0pyr,
     // Create a 2D occupancy grid for this current image
     // Note that we scale this down, so that each grid point is equal to a set of pixels
     // This means that we will reject points that less then grid_px_size points away then existing features
-    Eigen::MatrixXi grid_2d = Eigen::MatrixXi::Zero((int)(img0pyr.at(0).rows/min_px_dist)+2, (int)(img0pyr.at(0).cols/min_px_dist)+2);
+    // TODO: figure out why I need to add the windowsize of the klt to handle features that are outside the image bound
+    // TODO: I assume this is because klt of features at the corners is not really well defined, thus if it doesn't get a match it will be out-of-bounds
+    Eigen::MatrixXi grid_2d = Eigen::MatrixXi::Zero((int)(img0pyr.at(0).rows/min_px_dist)+15, (int)(img0pyr.at(0).cols/min_px_dist)+15);
     auto it0 = pts0.begin();
     auto it2 = ids0.begin();
     while(it0 != pts0.end()) {
@@ -386,8 +388,10 @@ void TrackKLT::perform_detection_stereo(const std::vector<cv::Mat> &img0pyr, con
     // Create a 2D occupancy grid for this current image
     // Note that we scale this down, so that each grid point is equal to a set of pixels
     // This means that we will reject points that less then grid_px_size points away then existing features
-    Eigen::MatrixXi grid_2d_0 = Eigen::MatrixXi::Zero((int)(img0pyr.at(0).rows/min_px_dist)+2, (int)(img0pyr.at(0).cols/min_px_dist)+2);
-    Eigen::MatrixXi grid_2d_1 = Eigen::MatrixXi::Zero((int)(img1pyr.at(0).rows/min_px_dist)+2, (int)(img1pyr.at(0).cols/min_px_dist)+2);
+    // TODO: figure out why I need to add the windowsize of the klt to handle features that are outside the image bound
+    // TODO: I assume this is because klt of features at the corners is not really well defined, thus if it doesn't get a match it will be out-of-bounds
+    Eigen::MatrixXi grid_2d_0 = Eigen::MatrixXi::Zero((int)(img0pyr.at(0).rows/min_px_dist)+15, (int)(img0pyr.at(0).cols/min_px_dist)+15);
+    Eigen::MatrixXi grid_2d_1 = Eigen::MatrixXi::Zero((int)(img1pyr.at(0).rows/min_px_dist)+15, (int)(img1pyr.at(0).cols/min_px_dist)+15);
     auto it0 = pts0.begin();
     auto it1 = ids0.begin();
     while(it0 != pts0.end()) {

diff --git a/ov_msckf/launch/pgeneva_ros_eth.launch b/ov_msckf/launch/pgeneva_ros_eth.launch
@@ -52,13 +52,14 @@
         <param name="record_timing_filepath"      type="string" value="$(arg path_time)" />
 
         <!-- tracker/extractor properties -->
-        <param name="use_klt"          type="bool"   value="true" />
-        <param name="num_pts"          type="int"    value="150" />
-        <param name="fast_threshold"   type="int"    value="15" />
-        <param name="grid_x"           type="int"    value="5" />
-        <param name="grid_y"           type="int"    value="3" />
-        <param name="min_px_dist"      type="int"    value="10" />
-        <param name="knn_ratio"        type="double" value="0.70" />
+        <param name="use_klt"            type="bool"   value="true" />
+        <param name="num_pts"            type="int"    value="150" />
+        <param name="fast_threshold"     type="int"    value="15" />
+        <param name="grid_x"             type="int"    value="5" />
+        <param name="grid_y"             type="int"    value="3" />
+        <param name="min_px_dist"        type="int"    value="10" />
+        <param name="knn_ratio"          type="double" value="0.70" />
+        <param name="downsample_cameras" type="bool"   value="false" />
 
         <!-- aruco tag/mapping properties -->
         <param name="use_aruco"        type="bool"   value="false" />

diff --git a/ov_msckf/launch/pgeneva_ros_tum.launch b/ov_msckf/launch/pgeneva_ros_tum.launch
@@ -52,13 +52,14 @@
         <param name="record_timing_filepath"      type="string" value="$(arg path_time)" />
 
         <!-- tracker/extractor properties -->
-        <param name="use_klt"          type="bool"   value="true" />
-        <param name="num_pts"          type="int"    value="200" />
-        <param name="fast_threshold"   type="int"    value="15" />
-        <param name="grid_x"           type="int"    value="5" />
-        <param name="grid_y"           type="int"    value="5" />
-        <param name="min_px_dist"      type="int"    value="5" />
-        <param name="knn_ratio"        type="double" value="0.65" />
+        <param name="use_klt"            type="bool"   value="true" />
+        <param name="num_pts"            type="int"    value="200" />
+        <param name="fast_threshold"     type="int"    value="15" />
+        <param name="grid_x"             type="int"    value="5" />
+        <param name="grid_y"             type="int"    value="5" />
+        <param name="min_px_dist"        type="int"    value="5" />
+        <param name="knn_ratio"          type="double" value="0.65" />
+        <param name="downsample_cameras" type="bool"   value="false" />
 
         <!-- aruco tag/mapping properties -->
         <param name="use_aruco"        type="bool"   value="false" />

diff --git a/ov_msckf/launch/pgeneva_ros_uzhfpv.launch b/ov_msckf/launch/pgeneva_ros_uzhfpv.launch
@@ -59,13 +59,14 @@
         <param name="record_timing_filepath"      type="string" value="$(arg path_time)" />
 
         <!-- tracker/extractor properties -->
-        <param name="use_klt"          type="bool"   value="true" />
-        <param name="num_pts"          type="int"    value="200" />
-        <param name="fast_threshold"   type="int"    value="15" />
-        <param name="grid_x"           type="int"    value="10" />
-        <param name="grid_y"           type="int"    value="8" />
-        <param name="min_px_dist"      type="int"    value="8" />
-        <param name="knn_ratio"        type="double" value="0.65" />
+        <param name="use_klt"            type="bool"   value="true" />
+        <param name="num_pts"            type="int"    value="200" />
+        <param name="fast_threshold"     type="int"    value="15" />
+        <param name="grid_x"             type="int"    value="10" />
+        <param name="grid_y"             type="int"    value="8" />
+        <param name="min_px_dist"        type="int"    value="8" />
+        <param name="knn_ratio"          type="double" value="0.65" />
+        <param name="downsample_cameras" type="bool"   value="false" />
 
         <!-- aruco tag/mapping properties -->
         <param name="use_aruco"        type="bool"   value="false" />

diff --git a/ov_msckf/launch/pgeneva_serial_aruco.launch b/ov_msckf/launch/pgeneva_serial_aruco.launch
@@ -45,13 +45,14 @@
         <param name="record_timing_filepath"      type="string" value="/tmp/traj_timing.txt" />
 
         <!-- tracker/extractor properties -->
-        <param name="use_klt"          type="bool"   value="true" />
-        <param name="num_pts"          type="int"    value="150" />
-        <param name="fast_threshold"   type="int"    value="15" />
-        <param name="grid_x"           type="int"    value="5" />
-        <param name="grid_y"           type="int"    value="3" />
-        <param name="min_px_dist"      type="int"    value="10" />
-        <param name="knn_ratio"        type="double" value="0.85" />
+        <param name="use_klt"            type="bool"   value="true" />
+        <param name="num_pts"            type="int"    value="150" />
+        <param name="fast_threshold"     type="int"    value="15" />
+        <param name="grid_x"             type="int"    value="5" />
+        <param name="grid_y"             type="int"    value="3" />
+        <param name="min_px_dist"        type="int"    value="10" />
+        <param name="knn_ratio"          type="double" value="0.85" />
+        <param name="downsample_cameras" type="bool"   value="false" />
 
         <!-- aruco tag/mapping properties -->
         <param name="use_aruco"        type="bool"   value="true" />

diff --git a/ov_msckf/launch/pgeneva_serial_eth.launch b/ov_msckf/launch/pgeneva_serial_eth.launch
@@ -46,13 +46,14 @@
         <param name="record_timing_filepath"      type="string" value="/tmp/timing_stereo.txt" />
 
         <!-- tracker/extractor properties -->
-        <param name="use_klt"          type="bool"   value="true" />
-        <param name="num_pts"          type="int"    value="150" />
-        <param name="fast_threshold"   type="int"    value="15" />
-        <param name="grid_x"           type="int"    value="5" />
-        <param name="grid_y"           type="int"    value="3" />
-        <param name="min_px_dist"      type="int"    value="10" />
-        <param name="knn_ratio"        type="double" value="0.70" />
+        <param name="use_klt"            type="bool"   value="true" />
+        <param name="num_pts"            type="int"    value="150" />
+        <param name="fast_threshold"     type="int"    value="15" />
+        <param name="grid_x"             type="int"    value="5" />
+        <param name="grid_y"             type="int"    value="3" />
+        <param name="min_px_dist"        type="int"    value="10" />
+        <param name="knn_ratio"          type="double" value="0.70" />
+        <param name="downsample_cameras" type="bool"   value="false" />
 
         <!-- aruco tag/mapping properties -->
         <param name="use_aruco"        type="bool"   value="false" />

diff --git a/ov_msckf/launch/pgeneva_serial_tum.launch b/ov_msckf/launch/pgeneva_serial_tum.launch
@@ -43,13 +43,14 @@
         <param name="record_timing_filepath"      type="string" value="/tmp/ov_msckf_timing.txt" />
 
         <!-- tracker/extractor properties -->
-        <param name="use_klt"          type="bool"   value="true" />
-        <param name="num_pts"          type="int"    value="200" />
-        <param name="fast_threshold"   type="int"    value="15" />
-        <param name="grid_x"           type="int"    value="5" />
-        <param name="grid_y"           type="int"    value="5" />
-        <param name="min_px_dist"      type="int"    value="5" />
-        <param name="knn_ratio"        type="double" value="0.65" />
+        <param name="use_klt"            type="bool"   value="true" />
+        <param name="num_pts"            type="int"    value="200" />
+        <param name="fast_threshold"     type="int"    value="15" />
+        <param name="grid_x"             type="int"    value="5" />
+        <param name="grid_y"             type="int"    value="5" />
+        <param name="min_px_dist"        type="int"    value="5" />
+        <param name="knn_ratio"          type="double" value="0.65" />
+        <param name="downsample_cameras" type="bool"   value="false" />
 
         <!-- aruco tag/mapping properties -->
         <param name="use_aruco"        type="bool"   value="false" />

diff --git a/ov_msckf/launch/pgeneva_sim.launch b/ov_msckf/launch/pgeneva_sim.launch
@@ -82,13 +82,14 @@
         <param name="feat_rep_aruco"         type="string" value="$(arg feat_rep)" />
 
         <!-- tracker/extractor properties -->
-        <param name="use_klt"          type="bool"   value="true" />
-        <param name="num_pts"          type="int"    value="$(arg num_pts)" />
-        <param name="fast_threshold"   type="int"    value="20" />
-        <param name="grid_x"           type="int"    value="5" />
-        <param name="grid_y"           type="int"    value="3" />
-        <param name="min_px_dist"      type="int"    value="8" />
-        <param name="knn_ratio"        type="double" value="0.70" />
+        <param name="use_klt"            type="bool"   value="true" />
+        <param name="num_pts"            type="int"    value="$(arg num_pts)" />
+        <param name="fast_threshold"     type="int"    value="20" />
+        <param name="grid_x"             type="int"    value="5" />
+        <param name="grid_y"             type="int"    value="3" />
+        <param name="min_px_dist"        type="int"    value="8" />
+        <param name="knn_ratio"          type="double" value="0.70" />
+        <param name="downsample_cameras" type="bool"   value="true" />
 
         <!-- aruco tag/mapping properties -->
         <param name="use_aruco"        type="bool"   value="false" />

diff --git a/ov_msckf/package.xml b/ov_msckf/package.xml
@@ -11,7 +11,6 @@
     <author email="[email protected]">Patrick Geneva</author>
     <author email="[email protected]">Kevin Eckenhoff</author>
     <maintainer email="[email protected]">Patrick Geneva</maintainer>
-    <maintainer email="[email protected]">Kevin Eckenhoff</maintainer>
 
     <!-- Licensing -->
     <license>GNU General Public License v3.0</license>
@@ -44,6 +43,7 @@
     <run_depend>cv_bridge</run_depend>
     <run_depend>ov_core</run_depend>
     <run_depend>ov_eval</run_depend>
+    <run_depend>ov_data</run_depend>
 
 
 </package>