diff --git a/src/test/gtest_vision_utils.cpp b/src/test/gtest_vision_utils.cpp
index c4bbee39dbe8e092983000ec0b97631bf00e85d6..a039c4f9c53616ae58e64701d2ccb21d674f5dce 100644
--- a/src/test/gtest_vision_utils.cpp
+++ b/src/test/gtest_vision_utils.cpp
@@ -376,6 +376,131 @@ TEST(VisionUtils, FeatureIdxGrid)
         result.push_back(elem.second);
     ASSERT_EQ(result.size(),1);
     ASSERT_EQ(result.at(0),1);
+
+
+    // Tracking grid
+    fg.clear();
+    // Check all are empty
+    for (unsigned int row=0;row<5;row++)
+        for (unsigned int col=0;col<5;col++)
+            ASSERT_TRUE(fg.pickEmptyTrackingCell(cell));
+
+    // Hit all by cell
+    for (unsigned int row=0;row<5;row++)
+    {
+        for (unsigned int col=0;col<5;col++)
+        {
+            cell << row, col;
+            fg.hitTrackingCell(cell);
+        }
+    }
+
+    // Check all are hit
+    for (unsigned int row=0;row<5;row++)
+        for (unsigned int col=0;col<5;col++)
+            ASSERT_FALSE(fg.pickEmptyTrackingCell(cell));
+
+    // Clear
+    fg.clear();
+    // Check all are empty
+    for (unsigned int row=0;row<5;row++)
+        for (unsigned int col=0;col<5;col++)
+            ASSERT_TRUE(fg.pickEmptyTrackingCell(cell));
+
+    // Hit all by [row, col]
+    for (unsigned int row=0;row<5;row++)
+        for (unsigned int col=0;col<5;col++)
+            fg.hitTrackingCell(row,col);
+
+    // Check all are hit
+    for (unsigned int row=0;row<5;row++)
+        for (unsigned int col=0;col<5;col++)
+            ASSERT_FALSE(fg.pickEmptyTrackingCell(cell));
+
+
+    // Clear
+    fg.clear();
+    // Check all are empty
+    for (unsigned int row=0;row<5;row++)
+        for (unsigned int col=0;col<5;col++)
+            ASSERT_TRUE(fg.pickEmptyTrackingCell(cell));
+
+    // Hit all by KeyPoint
+    for (unsigned int row=0;row<5;row++)
+    {
+        for (unsigned int col=0;col<5;col++)
+        {
+            cv::KeyPoint kp(row, col, CV_32F);
+            fg.hitTrackingCell(kp);
+        }
+    }
+
+    // Check all are hit
+    for (unsigned int row=0;row<5;row++)
+        for (unsigned int col=0;col<5;col++)
+            ASSERT_FALSE(fg.pickEmptyTrackingCell(cell));
+
+
+    // Block all (set to 0)
+    for (unsigned int row=0;row<5;row++)
+    {
+        for (unsigned int col=0;col<5;col++)
+        {
+            cell << row, col;
+            fg.blockTrackingCell(cell);
+        }
+    }
+
+    // Check all are blocked (empty)
+    for (unsigned int row=0;row<5;row++)
+        for (unsigned int col=0;col<5;col++)
+            ASSERT_TRUE(fg.pickEmptyTrackingCell(cell));
+
+    // Check hit + block
+    for (unsigned int row=0;row<5;row++)
+    {
+        for (unsigned int col=0;col<5;col++)
+        {
+            cell << row, col;
+            fg.hitTrackingCell(cell);
+            fg.blockTrackingCell(cell);
+        }
+    }
+
+    // Check all are blocked (empty)
+    for (unsigned int row=0;row<5;row++)
+        for (unsigned int col=0;col<5;col++)
+            ASSERT_TRUE(fg.pickEmptyTrackingCell(cell));
+
+    // Hit diagonal
+    fg.hitTrackingCell(0,0);
+    fg.hitTrackingCell(1,1);
+    fg.hitTrackingCell(2,2);
+    fg.hitTrackingCell(3,3);
+    fg.hitTrackingCell(4,4);
+
+    // Check the diagonal is not empty
+    for (unsigned int row=0;row<5;row++)
+        for (unsigned int col=0;col<5;col++)
+        {
+            bool empty = fg.pickEmptyTrackingCell(cell);
+            if (cell(0) == cell(1)) // diagonal must be hit
+                ASSERT_FALSE(empty);
+            else
+                ASSERT_TRUE(empty);
+        }
+
+    // block the diagonal
+    cell << 0,0; fg.blockTrackingCell(cell);
+    cell << 1,1; fg.blockTrackingCell(cell);
+    cell << 2,2; fg.blockTrackingCell(cell);
+    cell << 3,3; fg.blockTrackingCell(cell);
+    cell << 4,4; fg.blockTrackingCell(cell);
+
+    // Check all are blocked (empty)
+    for (unsigned int row=0;row<5;row++)
+        for (unsigned int col=0;col<5;col++)
+            ASSERT_TRUE(fg.pickEmptyTrackingCell(cell));
 }
 
 int main(int argc, char **argv) {