Video developer info

Doxyfile-common

@@ -98,7 +98,7 @@ INPUT                  = README.md \
 # For config item RECURSIVE, default is 0
 RECURSIVE              = YES
 
-IMAGE_PATH = doc
+IMAGE_PATH = doc plugins/videobasedtracker/doc
 MSCFILE_DIRS = doc
 
 # If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that

apps/sample-configs/osvr_server_config.VideoBasedHMDTracker.sample.json

@@ -1,11 +1,7 @@
 {
     "drivers": [{
         "plugin": "com_osvr_VideoBasedHMDTracker",
-        "driver": "VideoBasedHMDTracker",
-        "params": {
-            "cameraID": 0,
-            "showDebug": false
-        }
+        "driver": "VideoBasedHMDTracker"
     }],
     "plugins": [
         "com_osvr_VideoBasedHMDTracker" /* This is a manual-load plugin, so we must explicitly list it */

apps/sample-configs/osvr_server_config.VideoBasedHMDTrackerSingleSensor.sample.json

@@ -0,0 +1,109 @@
+{
+    "drivers": [{
+        "plugin": "com_osvr_VideoBasedHMDTracker",
+        "driver": "VideoBasedHMDTracker",
+        "params": {
+            "cameraID": 0,
+            "showDebug": false,
+            "solveIterations": 5,
+            "maxReprojectionAxisError": 4,
+            "sensors" : [
+                {
+                    "name": "OSVRHDKBothPanels",
+                    "requiredInliers": 4,
+                    "permittedOutliers": 0,
+                    "patterns": [
+                                ".**....*........"
+                              , "....**...*......"
+                              , ".*...**........."
+                              , ".........*....**"
+                              , "..*.....**......"
+                              , "*......**......."
+                              , "....*.*..*......"
+                              , ".*.*.*.........."
+                              , ".........*.**..."
+                              , "**...........*.."
+                              , "....*.*......*.."
+                              , "*.......*.*....."
+                              , ".*........*.*..."
+                              , ".*.........*.*.."
+                              , "..*..*.*........"
+                              , "....*...*.*....."
+                              , "...*.*........*."
+                              , "...*.....*.*...."
+                              , "....*......*..*."
+                              , "....*..*....*..."
+                              , "..*...*........*"
+                              , "........*..*..*."
+                              , ".......*...*...*"
+                              , "......*...*..*.."
+                              , ".......*....*..*"
+                              , "..*.....*..*...."
+                              , "*....*....*....."
+                              , "...*....*...*..."
+                              , "..*.....*...*..."
+                              , "...*......*...*."
+                              , "***...*........*"
+                              , "...****..*......"
+                              , "*.*..........***"
+                              , "**...........***"
+                              , "*...........**.."
+                              , "......**.*......"
+                              , ".............***"
+                              , "..........*....."
+                              , "...*.......**..."
+                              , "...**.....*....."
+                    ],
+                    "positions": [
+                            [-85, 3, 24.09],
+                            [-83.2, -14.01, 13.89],
+                            [-47, 51, 24.09],
+                            [47, 51, 24.09],
+                            [86.6, 2.65, 24.09],
+                            [85.5, -14.31, 13.89],
+                            [85.2, 19.68, 13.89],
+                            [21, 51, 24.09],
+                            [-21, 51, 24.09],
+                            [-84.2, 19.99, 13.89],
+                            [-60.41, 47.55, 44.6],
+                            [-80.42, 20.48, 42.9],
+                            [-82.01, 2.74, 42.4],
+                            [-80.42, -14.99, 42.9],
+                            [-60.41, -10.25, 48.1],
+                            [-60.41, 15.75, 48.1],
+                            [-30.41, 32.75, 50.5],
+                            [-31.41, 47.34, 47],
+                            [-0.41, -15.25, 51.3],
+                            [-30.41, -27.25, 50.5],
+                            [-60.44, -41.65, 45.1],
+                            [-22.41, -41.65, 47.8],
+                            [21.59, -41.65, 47.8],
+                            [59.59, -41.65, 45.1],
+                            [79.63, -14.98, 42.9],
+                            [29.59, -27.25, 50.5],
+                            [81.19, 2.74, 42.4],
+                            [79.61, 20.48, 42.9],
+                            [59.59, 47.55, 44.6],
+                            [30.59, 47.55, 47],
+                            [29.59, 32.75, 50.5],
+                            [-0.41, 20.75, 51.3],
+                            [59.59, 15.75, 48.1],
+                            [59.59, -10.25, 48.1],
+                            [-1, 23.8, -228.6],
+                            [-11, 5.8, -228.6],
+                            [-9, -23.8, -228.6],
+                            [0, -8.8, -228.6],
+                            [9, -23.8, -228.6],
+                            [12, 5.8, -228.6]
+                    ]
+                }
+            ]
+        }
+    }],
+    "plugins": [
+        "com_osvr_VideoBasedHMDTracker" /* This is a manual-load plugin, so we must explicitly list it */
+    ],
+    "aliases": {
+        "/me/head": "/com_osvr_VideoBasedHMDTracker/TrackedCamera0_0/semantic/OSVRHDKBothPanels"
+    }
+}

plugins/videobasedtracker/BeaconBasedPoseEstimator.cpp

@@ -89,13 +89,17 @@ namespace vbtracker {
     BeaconBasedPoseEstimator::BeaconBasedPoseEstimator(
         const DoubleVecVec &cameraMatrix, const std::vector<double> &distCoeffs,
         const Point3Vector &beacons, size_t requiredInliers,
-        size_t permittedOutliers) {
+        size_t permittedOutliers,
+        size_t solveIterations,
+        double maxReprojectionAxisError) {
         SetBeacons(beacons);
         SetCameraMatrix(cameraMatrix);
         SetDistCoeffs(distCoeffs);
         m_gotPose = false;
         m_requiredInliers = requiredInliers;
         m_permittedOutliers = permittedOutliers;
+        m_solveIterations = solveIterations;
+        m_maxReprojectionAxisError = maxReprojectionAxisError;
     }
 
     bool BeaconBasedPoseEstimator::SetBeacons(const Point3Vector &beacons) {
@@ -200,13 +204,12 @@ namespace vbtracker {
         // We tried using the previous guess to reduce the amount of computation
         // being done, but this got us stuck in infinite locations.  We seem to
         // do okay without using it, so leaving it out.
-        // @todo Make number of iterations into a parameter.
+        const float MaxReprojectionErrorForInlier = 87.0f;
         bool usePreviousGuess = false;
-        int iterationsCount = 5;
         cv::Mat inlierIndices;
         cv::solvePnPRansac(
             objectPoints, imagePoints, m_cameraMatrix, m_distCoeffs, m_rvec,
-            m_tvec, usePreviousGuess, iterationsCount, 8.0f,
+            m_tvec, usePreviousGuess, m_solveIterations, MaxReprojectionErrorForInlier,
             static_cast<int>(objectPoints.size() - m_permittedOutliers),
             inlierIndices);
 
@@ -220,7 +223,6 @@ namespace vbtracker {
         //==========================================================================
         // Reproject the inliers into the image and make sure they are actually
         // close to the expected location; otherwise, we have a bad pose.
-        const double pixelReprojectionErrorForSingleAxisMax = 4;
         if (inlierIndices.rows > 0) {
           std::vector<cv::Point3f>  inlierObjectPoints;
           std::vector<cv::Point2f> inlierImagePoints;
@@ -233,11 +235,11 @@ namespace vbtracker {
             m_distCoeffs, reprojectedPoints);
           for (size_t i = 0; i < reprojectedPoints.size(); i++) {
             if (reprojectedPoints[i].x - inlierImagePoints[i].x > 
-                pixelReprojectionErrorForSingleAxisMax) {
+              m_maxReprojectionAxisError) {
               return false;
             }
             if (reprojectedPoints[i].y - inlierImagePoints[i].y > 
-              pixelReprojectionErrorForSingleAxisMax) {
+              m_maxReprojectionAxisError) {
               return false;
             }
           }

plugins/videobasedtracker/BeaconBasedPoseEstimator.h

@@ -65,13 +65,21 @@ namespace vbtracker {
         /// @param distCoeffs Distortion coefficients for OpenCV
         /// @param beacons 3D beacon locations
         /// @param requiredInliers How many "good" points must be available
-        /// @param permittedOutliers How many additional "bad" points we can
-        /// have
+        /// @param permittedOutliers How many additional "bad" points we can have
+        /// @param solveIterations How many iterations to run the optimization algorithm for
+        /// @param maxReprojectionAxisError Maximum permitted reprojection error for a
+        ///        beacon that was identified in the image.  This is handled on a per-
+        ///        axis basis (it checks the maximum of X and Y reprojection error).
+        ///        It compares the image distance between the located beacon and the
+        ///        3D point reprojected into the image using the camera matrix.
+        ///        This is specified in camera pixels.
         BeaconBasedPoseEstimator(const DoubleVecVec &cameraMatrix,
                                  const std::vector<double> &distCoeffs,
                                  const Point3Vector &beacons,
                                  size_t requiredInliers = 4,
-                                 size_t permittedOutliers = 2);
+                                 size_t permittedOutliers = 2,
+                                 size_t solveIterations = 5,
+                                 double maxReprojectionAxisError = 4);
 
         /// @brief Produce an estimate of the pose of the model-space origin in
         /// camera space, where the origin is at the center of the image as
@@ -104,6 +112,8 @@ namespace vbtracker {
         cv::Mat m_distCoeffs;       //< Distortion coefficients
         size_t m_requiredInliers;   //< How many inliers do we require?
         size_t m_permittedOutliers; //< How many outliers do we allow?
+        size_t m_solveIterations;   //< How many iterations to run (at most) in solver
+        double m_maxReprojectionAxisError;  //< Maximum allowed reprojection error
 
         /// @name Pose cache
         /// @brief Stores the most-recent solution, in case we need it again

plugins/videobasedtracker/HDKLedIdentifier.cpp

@@ -59,19 +59,16 @@ namespace vbtracker {
                 return;
             }
 
-            // Make a new boolean-list encoding from it, replacing every
-            // non-'.' character with true and every '.' with false.
-            std::list<bool> pattern;
-            for (size_t j = 0; j < PATTERNS[i].size(); j++) {
-                if (PATTERNS[i][j] == '.') {
-                    pattern.push_back(false);
-                } else {
-                    pattern.push_back(true);
-                }
-            }
+            // Make a wrapped pattern, which is the original pattern plus
+            // a second copy of the pattern that has all but the last
+            // character in it.  This will enable use to use the string
+            // find() routine to see if any shift of the pattern is a
+            // match.
+            std::string wrapped = PATTERNS[i] + PATTERNS[i];
+            wrapped.pop_back();
 
             // Add the pattern to the vector of lists.
-            d_patterns.push_back(pattern);
+            d_patterns.push_back(wrapped);
         }
         // std::cout << "XXX d_length = " << d_length << ", num patterns = " <<
         // d_patterns.size() << std::endl;
@@ -108,26 +105,18 @@ namespace vbtracker {
         // pattern matches any of them.  If so, return that pattern.  We
         // need to check all potential rotations of the pattern, since we
         // don't know when the code started.  For the HDK, the codes are
-        // rotationally invariant.
-
-        /// @todo feels like there should be a good algorithm for
-        /// rotation-invariant string matching besides brute-forcing it here.
-        ///  -- rpavlik
+        // rotationally invariant.  We do this by making wrapped strings
+        // and seeing if the pattern shows up anywhe in them, relying on
+        // the std::string find method to do efficiently.
         for (size_t i = 0; i < d_patterns.size(); i++) {
-            for (size_t j = 0; j < bits.size(); j++) {
-                if (bits == d_patterns[i]) {
-                    return static_cast<int>(i);
-                }
-
-                // So long as we don't find the solution, this rotates
-                // back to the initial configuration after each inner loop.
-                std::list<bool>::iterator mid = bits.begin();
-                std::rotate(bits.begin(), ++mid, bits.end());
-            }
+          if (d_patterns[i].find(bits) != std::string::npos) {
+            return static_cast<int>(i);
+          }
         }
 
         // No pattern recognized and we should have recognized one, so return
-        // a low negative.  We've used -2 so return -3.
+        // a low negative.  We've used -2 for too-small brightness
+        // difference so return -3 for this.
         return -3;
     }
 

plugins/videobasedtracker/HDKLedIdentifierFactory.cpp

@@ -229,5 +229,9 @@ namespace vbtracker {
         return createHDKLedIdentifier(
             OsvrHdkLedIdentifier_RANDOM_IMAGES_PATTERNS);
     }
+
+    LedIdentifierPtr createSpecificLedIdentifier(PatternStringList patterns) {
+      return createHDKLedIdentifier(patterns);
+    }
 } // End namespace vbtracker
 } // End namespace osvr

plugins/videobasedtracker/HDKLedIdentifierFactory.h

@@ -51,6 +51,11 @@ namespace vbtracker {
     /// @brief Factory function to create an HDK Led Identifier object using the
     /// random images patterns.
     LedIdentifierPtr createRandomHDKLedIdentifier();
+
+    /// @brief Factory function to create a sensor identifier based on
+    /// a vector of strings.
+    LedIdentifierPtr createSpecificLedIdentifier(PatternStringList patterns);
+
 } // End namespace vbtracker
 } // End namespace osvr
 

plugins/videobasedtracker/IdentifierHelpers.h

@@ -62,17 +62,22 @@ namespace vbtracker {
     /// @brief Helper for implementations of LedIdentifier to turn a
     /// brightness list into a boolean list based on thresholding on the
     /// halfway point between minimum and maximum brightness.
-    inline LedPattern getBitsUsingThreshold(const BrightnessList &brightnesses,
-                                            float threshold) {
-        LedPattern ret;
+    inline LedPatternWrapped
+    getBitsUsingThreshold(const BrightnessList &brightnesses, float threshold) {
+        LedPatternWrapped ret;
         // Allocate output space for our transform.
         ret.resize(brightnesses.size());
 
-        // Transform the brightnesses into a container of bools using this
-        // little lambda
-        std::transform(
-            begin(brightnesses), end(brightnesses), begin(ret),
-            [threshold](Brightness val) { return val >= threshold; });
+        // Transform the brightnesses into a string with '.' for dim
+        // and '*' for bright.
+        std::transform(begin(brightnesses), end(brightnesses), begin(ret),
+                       [threshold](Brightness val) {
+                           if (val >= threshold) {
+                               return '*';
+                           } else {
+                               return '.';
+                           }
+                       });
 
         return ret;
     }

plugins/videobasedtracker/Types.h

@@ -50,11 +50,8 @@ namespace vbtracker {
 
     typedef std::vector<std::string> PatternStringList;
 
-    /// @todo std::list used here for ease of rotate, but has terrible data
-    /// locality - consider changing when a more efficient rotation-invariant
-    /// string match algorithm is used.
-    typedef std::list<bool> LedPattern;
-    typedef std::vector<LedPattern> PatternList;
+    typedef std::string LedPatternWrapped;  //< Pattern repeated almost twice
+    typedef std::vector<LedPatternWrapped> PatternList; //< Ordered set of patterns to search
 
     typedef std::vector<cv::KeyPoint> KeyPointList;
     typedef KeyPointList::iterator KeyPointIterator;
@@ -78,6 +75,19 @@ namespace vbtracker {
     typedef std::vector<LedGroup> LedGroupList;
     typedef std::vector<EstimatorPtr> EstimatorList;
     /// @}
+
+    /// Container for the information needed to define a sensor,
+    /// both the patterns of its beacons and their 3D spatial coordinates.
+    struct SensorDescription {
+      PatternStringList patterns;
+      Point3Vector positions;
+      size_t requiredInliers;
+      size_t permittedOutliers;
+      std::string name;
+    };
+    /// Description for a list of sensors.
+    typedef std::vector<SensorDescription> SensorDescriptionList;
+
 } // namespace vbtracker
 } // namespace osvr
 #endif // INCLUDED_Types_h_GUID_819757A3_DE89_4BAD_3BF5_6FE152F1EA08