Video developer info

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

@@ -4,7 +4,109 @@
         "driver": "VideoBasedHMDTracker",
         "params": {
             "cameraID": 0,
-            "showDebug": false
+            "showDebug": false,
+            "solveIterations": 5,
+            "maxReprojectionAxisError": 4,
+            "sensors" : [
+                {
+                    "name": "OSVRHDKFrontPanel",
+                    "requiredInliers": 6,
+                    "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]
+                    ]
+                },
+                {
+                    "name": "OSVRHDKBackPanel",
+                    "requiredInliers": 4,
+                    "permittedOutliers": 0,
+                    "patterns": [
+                                "*...........**.."
+                              , "......**.*......"
+                              , ".............***"
+                              , "..........*....."
+                              , "...*.......**..."
+                              , "...**.....*....."
+                    ],
+                    "positions": [
+                            [1, 23.8, 0],
+                            [11, 5.8, 0],
+                            [9, -23.8, 0],
+                            [0, -8.8, 0],
+                            [-9, -23.8, 0],
+                            [-12, 5.8, 0]
+                    ]
+                }
+            ]
         }
     }],
     "plugins": [

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/hmd/front"
+    }
+}

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) {
@@ -202,11 +206,10 @@ namespace vbtracker {
         // do okay without using it, so leaving it out.
         // @todo Make number of iterations into a parameter.
         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, 8.0f,
             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

@@ -66,12 +66,20 @@ namespace vbtracker {
         /// @param beacons 3D beacon locations
         /// @param requiredInliers How many "good" points must be available
         /// @param permittedOutliers How many additional "bad" points we can
+        /// @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.
         /// have
         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