diff --git a/Doxyfile-common b/Doxyfile-common index ed921ddaa..d9acf4778 100644 --- a/Doxyfile-common +++ b/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 diff --git a/apps/sample-configs/osvr_server_config.VideoBasedHMDTracker.sample.json b/apps/sample-configs/osvr_server_config.VideoBasedHMDTracker.sample.json index 41bf5dd83..194350c90 100644 --- a/apps/sample-configs/osvr_server_config.VideoBasedHMDTracker.sample.json +++ b/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 */ diff --git a/apps/sample-configs/osvr_server_config.VideoBasedHMDTrackerSingleSensor.sample.json b/apps/sample-configs/osvr_server_config.VideoBasedHMDTrackerSingleSensor.sample.json new file mode 100755 index 000000000..0c48be0c1 --- /dev/null +++ b/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" + } +} diff --git a/plugins/videobasedtracker/BeaconBasedPoseEstimator.cpp b/plugins/videobasedtracker/BeaconBasedPoseEstimator.cpp index bd9fafdea..15e98032a 100644 --- a/plugins/videobasedtracker/BeaconBasedPoseEstimator.cpp +++ b/plugins/videobasedtracker/BeaconBasedPoseEstimator.cpp @@ -89,13 +89,17 @@ namespace vbtracker { BeaconBasedPoseEstimator::BeaconBasedPoseEstimator( const DoubleVecVec &cameraMatrix, const std::vector &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(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 inlierObjectPoints; std::vector 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; } } diff --git a/plugins/videobasedtracker/BeaconBasedPoseEstimator.h b/plugins/videobasedtracker/BeaconBasedPoseEstimator.h index 559e7f8b4..c37b0265c 100644 --- a/plugins/videobasedtracker/BeaconBasedPoseEstimator.h +++ b/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 &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 diff --git a/plugins/videobasedtracker/HDKLedIdentifier.cpp b/plugins/videobasedtracker/HDKLedIdentifier.cpp index 33a9da36b..7dbbe72c3 100644 --- a/plugins/videobasedtracker/HDKLedIdentifier.cpp +++ b/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 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(i); - } - - // So long as we don't find the solution, this rotates - // back to the initial configuration after each inner loop. - std::list::iterator mid = bits.begin(); - std::rotate(bits.begin(), ++mid, bits.end()); - } + if (d_patterns[i].find(bits) != std::string::npos) { + return static_cast(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; } diff --git a/plugins/videobasedtracker/HDKLedIdentifierFactory.cpp b/plugins/videobasedtracker/HDKLedIdentifierFactory.cpp index 473a84b01..ddba97de7 100644 --- a/plugins/videobasedtracker/HDKLedIdentifierFactory.cpp +++ b/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 diff --git a/plugins/videobasedtracker/HDKLedIdentifierFactory.h b/plugins/videobasedtracker/HDKLedIdentifierFactory.h index 781fdee4b..91ab0b6cb 100644 --- a/plugins/videobasedtracker/HDKLedIdentifierFactory.h +++ b/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 diff --git a/plugins/videobasedtracker/IdentifierHelpers.h b/plugins/videobasedtracker/IdentifierHelpers.h index a9df7f7dd..a152ab869 100644 --- a/plugins/videobasedtracker/IdentifierHelpers.h +++ b/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; } diff --git a/plugins/videobasedtracker/Types.h b/plugins/videobasedtracker/Types.h index ace2d235b..ea10c2abd 100644 --- a/plugins/videobasedtracker/Types.h +++ b/plugins/videobasedtracker/Types.h @@ -50,11 +50,8 @@ namespace vbtracker { typedef std::vector 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 LedPattern; - typedef std::vector PatternList; + typedef std::string LedPatternWrapped; //< Pattern repeated almost twice + typedef std::vector PatternList; //< Ordered set of patterns to search typedef std::vector KeyPointList; typedef KeyPointList::iterator KeyPointIterator; @@ -78,6 +75,19 @@ namespace vbtracker { typedef std::vector LedGroupList; typedef std::vector 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 SensorDescriptionList; + } // namespace vbtracker } // namespace osvr #endif // INCLUDED_Types_h_GUID_819757A3_DE89_4BAD_3BF5_6FE152F1EA08 diff --git a/plugins/videobasedtracker/VideoBasedTracker.cpp b/plugins/videobasedtracker/VideoBasedTracker.cpp index 464b19bd7..af892dca3 100644 --- a/plugins/videobasedtracker/VideoBasedTracker.cpp +++ b/plugins/videobasedtracker/VideoBasedTracker.cpp @@ -44,9 +44,12 @@ namespace vbtracker { std::vector const &d, Point3Vector const &locations, size_t requiredInliers, - size_t permittedOutliers) { + size_t permittedOutliers, + size_t solveIterations, + double maxReprojectionAxisError) { addSensor(LedIdentifierPtr(identifier), m, d, locations, - requiredInliers, permittedOutliers); + requiredInliers, permittedOutliers, + solveIterations, maxReprojectionAxisError); } void VideoBasedTracker::addSensor(LedIdentifierPtr &&identifier, @@ -54,10 +57,13 @@ namespace vbtracker { std::vector const &d, Point3Vector const &locations, size_t requiredInliers, - size_t permittedOutliers) { + size_t permittedOutliers, + size_t solveIterations, + double maxReprojectionAxisError) { m_identifiers.emplace_back(std::move(identifier)); m_estimators.emplace_back(new BeaconBasedPoseEstimator( - m, d, locations, requiredInliers, permittedOutliers)); + m, d, locations, requiredInliers, permittedOutliers, + solveIterations, maxReprojectionAxisError)); m_led_groups.emplace_back(); m_assertInvariants(); } diff --git a/plugins/videobasedtracker/VideoBasedTracker.h b/plugins/videobasedtracker/VideoBasedTracker.h index 64d951d00..2677de431 100644 --- a/plugins/videobasedtracker/VideoBasedTracker.h +++ b/plugins/videobasedtracker/VideoBasedTracker.h @@ -70,13 +70,17 @@ namespace vbtracker { std::vector const &d, Point3Vector const &locations, size_t requiredInliers = 4, - size_t permittedOutliers = 2); + size_t permittedOutliers = 2, + size_t solveIterations = 5, + double maxReprojectionAxisError = 4); /// @overload void addSensor(LedIdentifierPtr &&identifier, DoubleVecVec const &m, std::vector const &d, Point3Vector const &locations, size_t requiredInliers = 4, - size_t permittedOutliers = 2); + size_t permittedOutliers = 2, + size_t solveIterations = 5, + double maxReprojectionAxisError = 4); /// @} typedef std::function diff --git a/plugins/videobasedtracker/com_osvr_VideoBasedHMDTracker.cpp b/plugins/videobasedtracker/com_osvr_VideoBasedHMDTracker.cpp index b9950bfad..88438b9d4 100644 --- a/plugins/videobasedtracker/com_osvr_VideoBasedHMDTracker.cpp +++ b/plugins/videobasedtracker/com_osvr_VideoBasedHMDTracker.cpp @@ -28,6 +28,7 @@ #include "HDKLedIdentifierFactory.h" #include #include +#include // Generated JSON header file #include "com_osvr_VideoBasedHMDTracker_json.h" @@ -85,7 +86,10 @@ namespace { class VideoBasedHMDTracker : boost::noncopyable { public: VideoBasedHMDTracker(OSVR_PluginRegContext ctx, CameraPtr &&camera, - int devNumber = 0, bool showDebug = false) + int devNumber = 0, bool showDebug = false, size_t solveIterations = 5, + double maxReprojectionAxisError = 4, + osvr::vbtracker::SensorDescriptionList sensors = + osvr::vbtracker::SensorDescriptionList()) #ifndef VBHMD_FAKE_IMAGES : m_camera(std::move(camera)) , m_vbtracker(showDebug) @@ -115,9 +119,57 @@ class VideoBasedHMDTracker : boost::noncopyable { /// Create an asynchronous (threaded) device m_dev.initAsync(ctx, os.str(), opts); - /// Send JSON descriptor - m_dev.sendJsonDescriptor(com_osvr_VideoBasedHMDTracker_json); + ///=============================================================== + /// Modify our JSON descriptor based on whether the configuration + /// parameters specify the sensors (in which case we change the number + /// to match) or whether they do not (in which case we add semantic + /// paths for the OSVR HDK, which is the default). + + // First read in and parse our compiled-in constant string + std::string myJsonString = osvr::util::makeString( + com_osvr_VideoBasedHMDTracker_json); + Json::Value myJson; + Json::Reader reader; + if (!reader.parse(myJsonString, myJson)) { + throw std::logic_error("Faulty JSON file for Video-based tracker - " + "should not be possible!"); + } + + // Next, adjust as needed and add new entries into the Json. + // We we are using the default case, we don't need to change + // anything. + Json::Value myFixedJson = myJson; + if (sensors.size() != 0) { + // We need to update the number of tracker sensors to match the + // number of sensors. + myFixedJson["interfaces"]["tracker"]["count"] = + static_cast(sensors.size()); + + // We need to build a new semantic entry for each of + // them that wraps the appropriate tracker sensor. We use the + // name specified in the sensor for the semantic name. + Json::Value mySemantic; + for (size_t i = 0; i < sensors.size(); i++) { + Json::Value mySensor; + + // Fill in the entries needed to get us to the tracker + std::string trackerSensor = "tracker/" + std::to_string(i); + mySensor["$target"]["child"] = trackerSensor; + + mySemantic[sensors[i].name] = mySensor; + } + + // Fill in the new semantic section. + myFixedJson["semantic"] = mySemantic; + } + + /// Add the semantic values into the fixed Json. + /// @todo + + /// Send adjusted JSON descriptor as our device descriptor + m_dev.sendJsonDescriptor(myFixedJson.toStyledString()); + ///=============================================================== /// Register update callback m_dev.registerUpdateCallback(this); @@ -145,7 +197,7 @@ class VideoBasedHMDTracker : boost::noncopyable { m_currentImage = 0; if (m_images.size() == 0) { - std::cerr << "Could not read any images from " << VBHMD_FAKE_IMAGES + std::cerr << "com_osvr_VideoBasedHMDTracker: Could not read any images from " << VBHMD_FAKE_IMAGES << std::endl; return; } @@ -191,12 +243,12 @@ class VideoBasedHMDTracker : boost::noncopyable { m_vbtracker.addSensor(osvr::vbtracker::createHDKLedIdentifierSimulated(0), m, d, osvr::vbtracker::OsvrHdkLedLocations_SENSOR0, 4, - 2); + 2, solveIterations, maxReprojectionAxisError); // There are sometimes only four beacons on the back unit (two of // the LEDs are disabled), so we let things work with just those. m_vbtracker.addSensor(osvr::vbtracker::createHDKLedIdentifierSimulated(1), m, d, osvr::vbtracker::OsvrHdkLedLocations_SENSOR1, 4, - 0); + 0, solveIterations, maxReprojectionAxisError); #else #ifdef VBHMD_USE_DIRECTSHOW @@ -221,6 +273,8 @@ class VideoBasedHMDTracker : boost::noncopyable { // See if this is an Oculus camera by checking the dimensions of // the image. This camera type improperly describes its format // as being a color format when it is in fact a mono format. + // @todo Check this by camera vendor name parameter added to config file + // rather than camera resolution found. bool isOculusCamera = (width == 376) && (height == 480); if (isOculusCamera) { m_type = OculusDK2; @@ -239,6 +293,8 @@ class VideoBasedHMDTracker : boost::noncopyable { //=============================================== // Configure objects and set up data structures and devices based on the // type of device we have. + // @todo separate this into a camera and a sensor portion, and base + // both off the config file rather than associated type. switch (m_type) { case OculusDK2: { @@ -304,12 +360,29 @@ class VideoBasedHMDTracker : boost::noncopyable { d.push_back(0); d.push_back(0); d.push_back(0); - m_vbtracker.addSensor( + + // If we have been passed a non-empty vector of sensors, we use these + // parameters when adding the senors. Otherwise, swap in those from + // the OSVR HDK. + if (sensors.size() == 0) { + m_vbtracker.addSensor( osvr::vbtracker::createHDKLedIdentifier(0), m, d, - osvr::vbtracker::OsvrHdkLedLocations_SENSOR0, 6, 0); - m_vbtracker.addSensor( + osvr::vbtracker::OsvrHdkLedLocations_SENSOR0, 6, 0, + solveIterations, maxReprojectionAxisError); + m_vbtracker.addSensor( osvr::vbtracker::createHDKLedIdentifier(1), m, d, - osvr::vbtracker::OsvrHdkLedLocations_SENSOR1, 4, 0); + osvr::vbtracker::OsvrHdkLedLocations_SENSOR1, 4, 0, + solveIterations, maxReprojectionAxisError); + } else { + for (size_t i = 0; i < sensors.size(); i++) { + m_vbtracker.addSensor( + osvr::vbtracker::createSpecificLedIdentifier(sensors[i].patterns), + m, d, + sensors[i].positions, + sensors[i].requiredInliers, sensors[i].permittedOutliers, + solveIterations, maxReprojectionAxisError); + } + } } break; @@ -375,7 +448,7 @@ class VideoBasedHMDTracker : boost::noncopyable { fileName << std::setfill('0') << std::setw(4) << m_imageNum++; fileName << ".tif"; if (!cv::imwrite(fileName.str().c_str(), m_frame)) { - std::cerr << "Could not write image to " << fileName.str() + std::cerr << "com_osvr_VideoBasedHMDTracker: Could not write image to " << fileName.str() << std::endl; } @@ -469,11 +542,17 @@ class VideoBasedHMDTracker : boost::noncopyable { class HardwareDetection { public: - HardwareDetection(int cameraID = 0, bool showDebug = false) + HardwareDetection(int cameraID = 0, bool showDebug = false, + size_t solveIterations = 5, double maxReprojectionAxisError = 4, + osvr::vbtracker::SensorDescriptionList sensors = + osvr::vbtracker::SensorDescriptionList()) : m_found(false) { m_cameraID = cameraID; m_showDebug = showDebug; + m_solveIterations = solveIterations; + m_maxReprojectionAxisError = maxReprojectionAxisError; + m_sensors = sensors; } OSVR_ReturnCode operator()(OSVR_PluginRegContext ctx) { @@ -505,7 +584,8 @@ class HardwareDetection { std::cout << "Opening camera " << m_cameraID << std::endl; osvr::pluginkit::registerObjectForDeletion( ctx, new VideoBasedHMDTracker(ctx, std::move(cam), - m_cameraID, m_showDebug)); + m_cameraID, m_showDebug, m_solveIterations, + m_maxReprojectionAxisError, m_sensors)); return OSVR_RETURN_SUCCESS; } @@ -517,6 +597,9 @@ class HardwareDetection { int m_cameraID; //< Which OpenCV camera should we open? bool m_showDebug; //< Show windows with video to help debug? + size_t m_solveIterations; //< How many iterations to run (at most) in solver + double m_maxReprojectionAxisError; //< Maximum allowed reprojection error + osvr::vbtracker::SensorDescriptionList m_sensors; //< Sensor descriptions (if available) }; class ConfiguredDeviceConstructor { @@ -529,7 +612,8 @@ class ConfiguredDeviceConstructor { if (params) { Json::Reader r; if (!r.parse(params, root)) { - std::cerr << "Could not parse parameters!" << std::endl; + std::cerr << "com_osvr_VideoBasedHMDTracker: Could not parse parameters!" << std::endl; + return OSVR_RETURN_FAILURE; } } @@ -539,11 +623,64 @@ class ConfiguredDeviceConstructor { // Using `get` here instead of `[]` lets us provide a default value. int cameraID = root.get("cameraID", 0).asInt(); bool showDebug = root.get("showDebug", false).asBool(); + size_t solveIterations = root.get("solveIterations", 5).asInt(); + double maxReprojectionAxisError = root.get("maxReprojectionAxisError", 4).asDouble(); + + // Default is an empty list, which can be filled in from the config + // file if we find entries there. + osvr::vbtracker::SensorDescriptionList mySensors; + if (root.isMember("sensors")) { + Json::Value sensors = root["sensors"]; + for (Json::ArrayIndex i = 0; i < sensors.size(); ++i) { + if ( !sensors[i].isMember("patterns") || + !sensors[i].isMember("positions") ) { + std::cerr << "com_osvr_VideoBasedHMDTracker: Expected patterns " + "and positions in sensor description for sensor" << i << std::endl; + return OSVR_RETURN_FAILURE; + } + + Json::Value patterns = sensors[i]["patterns"]; + Json::Value positions = sensors[i]["positions"]; + if (patterns.size() != positions.size()) { + std::cerr << "com_osvr_VideoBasedHMDTracker: Mismatched pattern count (" + << patterns.size() << ") and position count (" + << positions.size() << ") in sensor description for sensor" + << i << std::endl; + return OSVR_RETURN_FAILURE; + } + + // Read each pattern and position into the sensors list. + mySensors.push_back(osvr::vbtracker::SensorDescription()); + for (Json::ArrayIndex b = 0; b < patterns.size(); ++b) { + mySensors.back().patterns.push_back(patterns[b].asString()); + + Json::Value position = positions[b]; + if (position.size() != 3) { + std::cerr << "com_osvr_VideoBasedHMDTracker: Mismatched position size (" + << position.size() << "), expected 3, " + "in sensor description for sensor" << i + << ", beacon" << b << std::endl; + return OSVR_RETURN_FAILURE; + } + cv::Point3f v; + v.x = position[static_cast(0)].asFloat(); + v.y = position[static_cast(1)].asFloat(); + v.z = position[static_cast(2)].asFloat(); + mySensors.back().positions.push_back(v); + } + + // Fill in the other parameters + mySensors.back().name = sensors[i].get("name", "NULLSensorName").asString(); + mySensors.back().requiredInliers = sensors[i].get("requiredInliers", 4).asInt(); + mySensors.back().permittedOutliers = sensors[i].get("permittedOutliers", 0).asInt(); + } + } // OK, now that we have our parameters, create the device. osvr::pluginkit::PluginContext context(ctx); context.registerHardwareDetectCallback( - new HardwareDetection(cameraID, showDebug)); + new HardwareDetection(cameraID, showDebug, + solveIterations, maxReprojectionAxisError, mySensors)); return OSVR_RETURN_SUCCESS; } diff --git a/plugins/videobasedtracker/doc/Developing.md b/plugins/videobasedtracker/doc/Developing.md index 0ae840bf7..e8d183ac0 100644 --- a/plugins/videobasedtracker/doc/Developing.md +++ b/plugins/videobasedtracker/doc/Developing.md @@ -1,7 +1,175 @@ -## Coming Soon +# Developing for the OSVR video-based tracker -Sensics is moving the sensor codes and position information into the configuration file so that developers can add their own descriptions without recompiling. +This document describes how to develop new devices that will work with the OSVR video-based tracker, including how to design the flash patterns that should be used. See the [running document](./Running.md) for how to use the camera to track. -Also coming soon is a description of how the algorithm works and how to select codes that don't interfere with the existing HDK beacons. +## Brief theory of operation + +A *beacon* is light source that emits in the near infrared (an LED, for example). A *sensor* is a device that has four or more beacons rigidly mounted on its surface. + +The OSVR video-based tracking system tracks one or more sensors. The beacons on each sensor flash in synchronization with the video camera, controlled by a digital signal the comes from the camera itself. On the OSVR HDK, this signal plugs in using a Y adapter to the power cord (as shown below). + +![Synchronized power plug](./sync_power_adapter.png) + +The camera takes images at about 100 frames/second. Each beacon strobes briefly during this time, with the brightness of each consecutive strobe modulated to match a 16-frame pattern. The exact pattern is detailed below, but each beacon basically has two possible brightnesses for each frame, which we'll call *dim* and *bright*. Each beacon has a unique 16-frame pattern, which is used to distinguish it from other beacons. (Note that lights, other LEDs in the environment, and the sun will produce spots on the image but they will not strobe with a specific pattern, so are not recognized as beacons by the tracking system.) + +The video-based tracking system is provided with a description for each sensor that includes the strobe patterns of its beacons and also the 3D positions where they are mounted. It also has a model of the camera's distortion. It uses the flashing pattern to identify the 2D screen-space positions of each beacon that is visible. It then finds an error-minimizing pose in 3-space for those beacons to minimize the error of the 3D positions projected through the camera model back onto the screen. When enough beacons are seen and the error is small enough, it produces a report telling the pose of the sensor with respect to the camera's center of projection and orientation. + +## New sensor design + +New sensors that are to be compatible with the OSVR video-based tracking system must have beacon flash patterns and layouts that are consistent with the system and which do not include beacons whose flash patterns that match any other beacons in the system, in particular the exiting 40 LEDs on the front and back panels of the OSVR HDK (whose patterns are listed in the appendix). + +### Flash patterns + +The patterns used are rotationally invariant. The detection algorithms used in the system act independently on each beacon to determine the pattern. This enables multiple sensors to operate at an arbitary phase relative to one another while still having their beacons recognized. **This means that the strobe patterns of all beacons must be unique under all rotations -- shifting an existing pattern one or more frames later does not produce a new pattern.** Therefore any new patterns introduced must be distinct compared to **all rotations** of existing patterns. + +#### Available patterns + +The OSVR HDK uses all of the 16-bit, odd-parity patterns with 1 and 3 bright flashes, and four of the patterns with 5 bright flashes. Patterns with seven or more bright flashes are not currently being used. Maintaining good error characteristics in the presence of multiple devices will require choosing odd parities for all devices. There are 2048 total odd-parity codes. + +Relaxing the constraint that single-bit errors must result in invalid codes would allow the use of even-parity codes. None of these codes are currently in use. There are 2067 total even-parity codes. Single-bit errors can convert even-parity codes into odd-parity codes. + +A program that can determine the set of rotationally-invariant patterns that has the smallest maximum instantaneous power draw can be found [in this repository](https://github.com/sensics/LED_encoding). + +#### ToDo: Determine how to allocate codes to vendors + +### Beacon layout + +![Video debug when working](./video_debug.png) + +The beacons should be arranged in space such that at least four beacons can be seen largely front-on from any angle that the sensor should be tracked. The image above shows the beacon layout for one view of the OSVR HDK. There are several criteria for placement, some of which are at odds with one another: + +* **Visibility:* There should be mostly head-on views of at least four beacons from any viewpoint. Note that there are beacons on the sides, top, and bottom of the HDK that become face-on as the unit is rotated. +* **Separation:** As the sensor moves further from the camera, the beacons come closer together in the image. As they do so, their bright regions get closer together, which reduces the baseline for measurement (increasing the angular error in the estimates) and eventually causes them to overlap (losing identification of both beacons). +* **Generality:** The pose-estimation algorithms require the poses to be generic. It does not help to see four beacons if they are in a degenerate configuration where they all lie along the same line because the rotation about that line cannot be determined. +* **Rigidity:** The relative positions of the beacons must remain fixed relative to each other as the sensor is moved. Flexible objects or beacons mounted on slender portions may move relative to one another, introducing errors in the pose estimation. + +### Config files + +The OSVR video-based tracker is run using [json](http://www.json.org/)-formatted configuration files that can include descriptions of the sensors it should be looking for, including their flash patterns and 3D positions. It is also possible to adjust parameters of the algorithm from within the configuration file, enabling peformance tuning without recompilation. A configuration file that handles just the back-plate sensor on the OSVR HDK is shown below, with a description of its entries following. + + { + "drivers": [{ + "plugin": "com_osvr_VideoBasedHMDTracker", + "driver": "VideoBasedHMDTracker", + "params": { + "showDebug": true, + "solveIterations": 5, + "maxReprojectionAxisError": 4, + "sensors" : [ + { + "name": "OSVRHDKBack", + "requiredInliers": 4, + "permittedOutliers": 0, + "patterns": [ + "*...........**.." + , "......**.*......" + , ".............***" + , "..........*....." + , "...*.......**..." + , "...**.....*....." + ], + "positions": [ + [-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/OSVRHDKBack" + } + } + +The portion of the config file relevant to the development of new sensors is in the **"params"** section. The meaning of each parameter is as follows: +* **showDebug**: This parameter controls whether a debuggin window appears showing the video image, identified beacons (in red) and identified sensors (whose beacons become green). The [running document](./Running.md) describes how to use this window and what is shown in it. +* **solveIterations**: This controls the maximum number of iterations performed by the OpenCV optimization algorithm to determine the optimal pose. Increasing this can improve tracking accuracy, but it also uses more CPU power and too-large values will eventually reduce tracking rate so much that the system can no longer identify beacons. +* **maxReprojectionError**: This parameter is used to discard bad poses that can cause jitter in the tracking reports. It is the maximum distance in pixels that either axis (x or y) can vary between the location of a beacon identified in the image and its reprojected 3D location from the model. +* **sensors**: This section provides a list of available sensors in the system. There can be more than one sensor described, but this example shows only one. Each sensor is reported with a differnt ID by the video-based tracker. The first listed sensor is given ID 0. + +Within each sensor, there are several fields: + +* **name**: Descriptive name of the sensor, set as the semantic name. +* **requiredInliers**: How many beacons must be identified before the system attempts to report a pose. For the OSVR HDK rear panel, some units only have four of the six sensors visible, so this is chosen as 4. For the front panel on the HDK, the default configuration requires at least six beacons to be visible to reduce jitter in the reported poses. +* **permittedOutliers**: Reflections of beacons off of specular surfaces, extra lights in the scene, and mis-identified beacons can produce completely wrong estimates for one or a few beacons in the scene. This parameter specifies how many "outlier" responses are allowed, where the beacon complely mis-matches the expected location. In this case, the beacon is completely ignored. In the case of 4 inliers, all are needed. +* **patterns**: There is one pattern for each beacon on the sensor. These must be listed in the same order as the **positions** parameter described below. All of the patterns must be of the same length for a given sensor. If sensors with different pattern lengths are visible together, **all shorter-length subsets of the longer patterns must be distinct from all rotations of all of the shorter patterns present in the scene**. The rotation of each pattern is not important, because it matches all rotations. The pattern is encoded using a period ('.') for each dim flash and an asterisk ('*') for each bright flash. +* **positions**: There is one position for each beacon on the sensor. These must be listed in the same order as the **patterns** parameter described above. This is the position in millimeters of each object on the sensor. These can be defined in any consistent coordinate system. The origin of that coordinate system becomes the origin of the sensor as reported by the video-based tracking system. The orientation of that coordinate system becomes the orientation reported. + +## Appendix: Design criteria and resulting coding for OSVR HDK + +The assumptions on the system characteristics, which must be satisfied for the proposed design to work, are given first. Next come the criteria being optimized, from most important to least. Different prioritizations for criteria result in different optimal coding algorithms (for example, it is possible to achieve a single bright LED at any given time for a non-error-detecting code using information across LEDs by making a code that is 50 frames long, but this would require almost a second to determine LED identity for a camera operating at 60fps and would require seeing all LEDs). + +### Assumptions +* The LED flashing will be synchronized with the camera frames such that they are on high-power or low-power for the duration of exposure for consecutive frames. This will allow the computer-vision algorithm to reliably detect data values at full frame rate. +* The brightness difference between high power and low power is sufficient to be reliably detected, removing the need for error correcting codes. + +### Criteria + +* Make a code that is independent, so that each LED can be treated on its own. **Solution:** Rotationally-invariant code selected. +* Minimize the maximum instantaneous power draw over all LEDs. **Approach:** A set of encodings with the minimum number of 1’s, subject to being rotationally invariant, was selected. An iterative optimization algorithm was run to select the best set of rotations, which matched the theoretical minimum number of simultaneous LEDs. +* Minimize time to determine LED identities for 40 LEDs. **Approach:** A subset of the binary code is selected, where the binary code is the most-efficient encoding of bits. +* Provide an error check so that single-bit errors can be detected. **Approach:** The set of encodings selected for the HDK has odd parity, so any single-bit error will appear as an invalid code. + +### Design + +Each LED pattern consists of an infinitely-repeated series of frames. Each frame consists of a series of bright(1)/dim(0) bits lasting for one camera frame, synchronized with the camera exposures. + +There are N bits within a frame (N=16 was selected). See below for the LED pattern tables. The patterns have the characteristic that no patterns can be rotated to produce another so that the ID for a particular LED can be reliably determined by looking only at that LED’s sequence. The relative start time of the transmissions for different LEDs does not matter. Patterns with even, odd, and no parity checking were tested, to see the impact of enabling parity checking to detect single-bit errors. + +This forms a family of encodings parameterized by N (the number of bits used to encode) and parity. As N increases, the time to determine a complete encoding increases but the maximum number of overlapping LEDs in the high state decreases. The minimum counts with optimal packing was computed for a range of choices and a size of 16 with odd parity was selected. + +#### Encoding + +The encoding selected for the OSVR HDK is shown below. Each period ('.') indicates a dim flash and each asterisk ('*') indicates a bright flash. The LED index is listed along the left edge (this does not match the final hardware ordering in the as-designed unit). + + 0: ***...*........* + 1: ...****..*...... + 2: *.*..........*** + 3: **...........*** + 4: *....*....*..... + 5: ...*....*...*... + 6: ..*.....*...*... + 7: ...*......*...*. + 8: .......*...*...* + 9: ......*...*..*.. + 10: .......*....*..* + 11: ..*.....*..*.... + 12: ....*......*..*. + 13: ....*..*....*... + 14: ..*...*........* + 15: ........*..*..*. + 16: ..*..*.*........ + 17: ....*...*.*..... + 18: ...*.*........*. + 19: ...*.....*.*.... + 20: ....*.*......*.. + 21: *.......*.*..... + 22: .*........*.*... + 23: .*.........*.*.. + 24: ....*.*..*...... + 25: .*.*.*.......... + 26: .........*.**... + 27: **...........*.. + 28: .*...**......... + 29: .........*....** + 30: ..*.....**...... + 31: *......**....... + 32: ...*.......**... + 33: ...**.....*..... + 34: .**....*........ + 35: ....**...*...... + 36: *...........**.. + 37: ......**.*...... + 38: .............*** + 39: ..........*..... + +To minimize the maximum instantaneous power, the patterns have been optimally rotated. A count of the number of LEDs on per time step is: 8 8 8 8 8 7 8 7 8 8 8 8 8 8 8 8.