diff --git a/AprilTagTrackers/IPC/WindowsNamedPipe.cpp b/AprilTagTrackers/IPC/WindowsNamedPipe.cpp index 0ad22344..e99b9c58 100644 --- a/AprilTagTrackers/IPC/WindowsNamedPipe.cpp +++ b/AprilTagTrackers/IPC/WindowsNamedPipe.cpp @@ -21,14 +21,14 @@ std::string_view WindowsNamedPipe::SendRecv(std::string message) // NOLINTNEXTLINE: Remove const-ness as callnamedpipe expects a void*, but it will not be modified LPVOID messagePtr = reinterpret_cast(const_cast(message.data())); DWORD responseLength = 0; - if (FAILED(CallNamedPipeA( + if ((CallNamedPipeA( //the FAILED() macro checks whether return was nonzero, but CallNamedPipeA returns 0 on fail mPipeName.c_str(), messagePtr, message.size() + 1, GetBufferPtr(), GetBufferSize(), &responseLength, - 2 * SEC_TO_MS))) + 2 * SEC_TO_MS)) == 0) { ATT_LOG_ERROR("named pipe send error: ", GetLastError()); throw std::system_error(static_cast(GetLastError()), std::system_category()); diff --git a/AprilTagTrackers/Tracker.cpp b/AprilTagTrackers/Tracker.cpp index 329836bc..39c9207d 100644 --- a/AprilTagTrackers/Tracker.cpp +++ b/AprilTagTrackers/Tracker.cpp @@ -534,7 +534,16 @@ void Tracker::StartTrackerCalib() void Tracker::StartConnection() { - mVRDriver = tracker::VRDriver{user_config.trackers}; + try + { + mVRDriver = tracker::VRDriver{user_config.trackers}; + } + catch (const std::exception& e) + { + ATT_LOG_ERROR(e.what()); + gui->ShowPopup(lc.CONNECT_DRIVER_ERROR, PopupStyle::Error); //TODO: error code is no longer returned in messsage, but is available in log, so error message should be changed + return; + } if (!user_config.disableOpenVrApi) { mVRClient = std::make_unique(); @@ -543,12 +552,22 @@ void Tracker::StartConnection() { mVRClient = std::make_unique(); } - if (!mVRClient->CanInit()) + if (!mVRClient->CanInit()) //CanInit() does not seem to care whether hmd is connected or not. This step may not be necessary { gui->ShowPopup("Unable to initialize steamvr client, is your hmd connected?", PopupStyle::Error); return; } - mVRClient->Init(); + try + { + mVRClient->Init(); + } + catch (const std::exception& e) + { + ATT_LOG_ERROR(e.what()); + gui->ShowPopup(lc.CONNECT_CLIENT_ERROR, PopupStyle::Error); //TODO: error code is no longer returned in messsage, but is available in log, so error message should be changed + return; + } + gui->SetStatus(true, StatusItem::Driver); } @@ -593,29 +612,30 @@ void Tracker::Start() { gui->ShowPopup(lc.TRACKER_CAMERA_NOTRUNNING, PopupStyle::Error); mainThreadRunning = false; - mainThread.join(); + //mainThread.join(); this causes crash if main thread is not running. Shouldnt be neccesary since, when it is running, previous check will already return? return; } if (calib_config.cameras[0]->cameraMatrix.empty()) { gui->ShowPopup(lc.TRACKER_CAMERA_NOTCALIBRATED, PopupStyle::Error); mainThreadRunning = false; - mainThread.join(); + //mainThread.join(); return; } if (!IsTrackerUnitsCalibrated()) { gui->ShowPopup(lc.TRACKER_TRACKER_NOTCALIBRATED, PopupStyle::Error); mainThreadRunning = false; - mainThread.join(); + //mainThread.join(); return; } - if (!mVRClient->IsInit() || !mVRDriver) + + if (!mVRClient || !mVRClient->IsInit() || !mVRDriver) { gui->ShowPopup(lc.TRACKER_STEAMVR_NOTCONNECTED, PopupStyle::Error); mainThreadRunning = false; - mainThread.join(); - return; + //mainThread.join(); + //return; ALLOW NO CONNECTION FOR TESTING PURPOSES. UNCOMMENT LATER } gui->SetStatus(true, StatusItem::Tracker); @@ -799,14 +819,90 @@ void Tracker::CalibrateTracker() void Tracker::MainLoop() { - tracker::MainLoopRunner runner(&user_config, &calib_config, &mPlayspace, &mVRDriver.value()); + //initializing variables used as communication between modules + tracker::CapturedFrame frame{}; + cv::Mat drawImg{}; + cv::Mat workImg{}; + MarkerDetectionList dets{}; + + tracker::DummyDriver * driver = new tracker::DummyDriver{user_config.trackers}; + //tracker::VRDriver* driver = + + //initializing all analysis module classes + //tracker::MainLoopRunner runner(&user_config, &calib_config, &mPlayspace, &mVRDriver.value()); + + std::unique_ptr getPose; + std::unique_ptr preprocess; + std::unique_ptr detect; + std::unique_ptr estimatePose; + std::unique_ptr sendPose; + std::unique_ptr draw; + + std::unique_ptr mCalibrator; + + try + { + //TODO: instead of passsing gui, every gui accesss should probably be done through the ITrackerControl interface + getPose = std::make_unique(&user_config, &mVRDriver.value(), gui); + preprocess = std::make_unique(&user_config, &mVRDriver.value(), gui); + detect = std::make_unique(&user_config, &mVRDriver.value(), gui); + estimatePose = std::make_unique(&user_config, &mVRDriver.value(), gui); + sendPose = std::make_unique(&user_config, &mVRDriver.value(), gui); + draw = std::make_unique(&user_config, &mVRDriver.value(), gui); + + mCalibrator = std::make_unique(); + } + catch (const std::exception& e) + { + ATT_LOG_ERROR(e.what()); + gui->ShowPopup(lc.CONNECT_SOMETHINGWRONG, PopupStyle::Error); //should use a generic somethingwrong message + return; + } // run detection until camera is stopped or the start/stop button is pressed again while (mainThreadRunning && cameraRunning) { try { - runner.Update(&mCameraFrame, gui, &mTrackerUnits, mVRClient.get(), this); + //0. await for latast frame and save to an image to work on and image to draw on + //NOTE: this step does too many data copies, and should be changed in the future. Especialy since grayscaling image already does a copy. + mCameraFrame.Get(frame); + drawImg = frame.image.clone(); + workImg = frame.image.clone(); + //1. get latest data from driver + //getPose->Update(&frame, &workImg, &drawImg, &dets, &mTrackerUnits); + //2. preprocess the frame. This includes grayscaling and masking. + preprocess->Update(&frame, &workImg, &drawImg, &dets, &mTrackerUnits); + //3. run detection on frame using selected library + detect->Update(&frame, &workImg, &drawImg, &dets, &mTrackerUnits); + //4. run pose estimation on detections using calibrated tracker data + estimatePose->Update(&frame, &workImg, &drawImg, &dets, &mTrackerUnits); + //6. send stuff to steamvr + sendPose->Update(&frame, &workImg, &drawImg, &dets, &mTrackerUnits); + //7. draw and show preview + draw->Update(&frame, &workImg, &drawImg, &dets, &mTrackerUnits); + + //cv::aruco::drawDetectedMarkers(drawImg, dets.corners, dets.ids, cv::Scalar(255, 0, 0)); + //cv::imshow("out", workImg); + //cv::waitKey(1); + + //runner.Update(&mCameraFrame, gui, &mTrackerUnits, mVRClient.get(), this); + + //run calibration steps. TODO: slight reformat to be more in line with above steps + + mCalibrator->Update(mVRClient, driver, gui, &mPlayspace, lockHeightCalib, manualRecalibrate); + for (int index = 0; index < mTrackerUnits.size(); ++index) + { + //draw pose from driver if available, else draw pose as detected + auto& unit = (mTrackerUnits)[index]; + if (multicamAutocalib && unit.WasVisibleToDriverLastFrame()) + { + tracker::PlayspaceCalibrator::UpdateMulticam(gui, &mPlayspace, unit); + //TODO: should skip sending to driver, but that cannot be done with this setup. + //should the entire sendPose step be skipped when multicamAutocalib is activated? + } + } + } catch (const std::exception& e) { diff --git a/AprilTagTrackers/Tracker.hpp b/AprilTagTrackers/Tracker.hpp index f3d0adbd..d0d9f047 100644 --- a/AprilTagTrackers/Tracker.hpp +++ b/AprilTagTrackers/Tracker.hpp @@ -22,6 +22,8 @@ #include #include +#include "utils/Error.hpp" + struct TrackerStatus { cv::Vec3d boardRvec, boardTvec, boardTvecDriver; diff --git a/AprilTagTrackers/tracker/MainLoopRunner.hpp b/AprilTagTrackers/tracker/MainLoopRunner.hpp index 76b8fc04..21f9a33d 100644 --- a/AprilTagTrackers/tracker/MainLoopRunner.hpp +++ b/AprilTagTrackers/tracker/MainLoopRunner.hpp @@ -11,6 +11,482 @@ namespace tracker { +static constexpr int DRAW_IMG_SIZE = 480; // TODO: make configurable (preview image scaler) +static inline const cv::Scalar COLOR_MASK{255, 0, 0}; /// red + +class Draw +{ +private: + utils::SteadyTimer detectionTimer{}; + int framesSinceLastSeen = 0; + static constexpr int framesToCheckAll = 20; + cv::Mat maskSearchImg{}; + cv::Mat tempGrayMaskedImg{}; + + RefPtr mConfig; + RefPtr camCalib; + RefPtr videoStream; + AprilTagWrapper april; + Index trackerNum; + RefPtr mPlayspace; + RefPtr mVRDriver; + RefPtr gui; + RefPtr> trackerUnits; + RefPtr vrClient; + RefPtr trackerCtrl; + +public: + explicit Draw(RefPtr config, + RefPtr vrDriver, + RefPtr gui) + + : mConfig(config), + camCalib(mConfig->calib.cameras[0]), + videoStream(mConfig->videoStreams[0]), + april(AprilTagWrapper::ConvertFamily(mConfig->markerLibrary), videoStream->quadDecimate, mConfig->apriltagThreadCount), + trackerNum(mConfig->trackerNum), + mVRDriver(vrDriver), + gui(gui) + + {} + + //the way image is passed should change, probably no use having 3 args for it + void Update(RefPtr frame, + RefPtr workImg, + RefPtr drawImg, + RefPtr dets, + RefPtr> trackerUnits) + { + if (gui->IsPreviewVisible()) + { + const double frameTimeAfterDetect = duration_cast(utils::SteadyTimer::Now() - frame->timestamp).count(); + // draw and display the detections + if (!dets->ids.empty()) cv::aruco::drawDetectedMarkers(*drawImg, dets->corners, dets->ids); + + for (int index = 0; index < trackerUnits->size(); ++index) + { + //draw pose from driver if available, else draw pose as detected + auto& unit = (*trackerUnits)[index]; + if (unit.WasVisibleToDriverLastFrame()) + cv::drawFrameAxes(*drawImg, camCalib->cameraMatrix, camCalib->distortionCoeffs, unit.GetPoseFromDriver().rotation.value, unit.GetPoseFromDriver().position, 0.1); + else if (unit.WasVisibleLastFrame()) + cv::drawFrameAxes(*drawImg, camCalib->cameraMatrix, camCalib->distortionCoeffs, unit.GetEstimatedPose().rotation.value, unit.GetEstimatedPose().position, 0.1); + } + + const cv::Size2i drawSize = ConstrainSize(GetMatSize(frame->image), DRAW_IMG_SIZE); + cv::resize(*drawImg, *drawImg, drawSize); + cv::putText(*drawImg, std::to_string(frameTimeAfterDetect).substr(0, 5), cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 1, cv::Scalar(255, 255, 255)); + if (false) // TODO: tracker->showTimeProfile (is this even needed?) + { + april.DrawTimeProfile(*drawImg, cv::Point(10, 60)); + } + gui->UpdatePreview(*drawImg); + } + } +}; + +class SendPose +{ +private: + + RefPtr mConfig; + RefPtr videoStream; + PlayspaceCalib mPlayspace; + RefPtr mVRDriver; + RefPtr> trackerUnits; + +public: + explicit SendPose(RefPtr config, + RefPtr vrDriver, + RefPtr gui) + + : mConfig(config), + videoStream(mConfig->videoStreams[0]), + mVRDriver(vrDriver) + + { + mPlayspace.Set(mConfig->manualCalib.GetAsReal()); + } + + //the way image is passed should change, probably no use having 3 args for it + void Update(RefPtr frame, + RefPtr workImg, + RefPtr drawImg, + RefPtr dets, + RefPtr> trackerUnits) + { + const double frameTimeAfterDetect = duration_cast(utils::SteadyTimer::Now() - frame->timestamp).count(); + for (int index = 0; index < trackerUnits->size(); ++index) + { + auto& unit = (*trackerUnits)[index]; + + // transform boards position based on our calibration data + Pose poseToSend = mPlayspace.TransformToOVR(Pose(unit.GetEstimatedPose())); + + // send all the values + mVRDriver->UpdateTracker(index, poseToSend, -frameTimeAfterDetect - videoStream->latency, mConfig->smoothingFactor); + } + + } +}; + +class GetPose +{ +private: + + RefPtr mConfig; + RefPtr camCalib; + RefPtr videoStream; + Index trackerNum; + PlayspaceCalib mPlayspace; + RefPtr mVRDriver; + RefPtr> trackerUnits; + +public: + explicit GetPose(RefPtr config, + RefPtr vrDriver, + RefPtr gui) + + : mConfig(config), + camCalib(mConfig->calib.cameras[0]), + videoStream(mConfig->videoStreams[0]), + trackerNum(mConfig->trackerNum), + mVRDriver(vrDriver) + + { + mPlayspace.Set(mConfig->manualCalib.GetAsReal()); + } + + //the way image is passed should change, probably no use having 3 args for it + void Update(RefPtr frame, + RefPtr workImg, + RefPtr drawImg, + RefPtr dets, + RefPtr> trackerUnits) + { + for (int i = 0; i < trackerNum; i++) + { + //querry the driver for every connected tracker unit + + auto& unit = (*trackerUnits)[i]; + + const double frameTimeSinceCapture = duration_cast(utils::SteadyTimer::Now() - frame->timestamp).count(); + auto [pose, isValid] = mVRDriver->GetTracker(i, -frameTimeSinceCapture - videoStream->latency); + + //convert pose to local space + if (isValid) + pose = mPlayspace.InvTransformFromOVR(pose); + + //if pose was valid, set the PoseFromDriver parameter of tracker unit + unit.SetWasVisibleToDriverLastFrame(isValid); + if (isValid) // if the pose from steamvr was valid, save the predicted position and rotation + { + unit.SetWasVisibleLastFrame(true); + unit.SetPoseFromDriver(RodrPose(pose)); + } + } + } +}; + +class EstimatePose +{ +private: + + RefPtr mConfig; + RefPtr camCalib; + Index trackerNum; + PlayspaceCalib mPlayspace; + RefPtr> trackerUnits; + //TODO: set a way for interaction with gui, here trackerCtrl + RefPtr trackerCtrl; + +public: + explicit EstimatePose(RefPtr config, + RefPtr vrDriver, + RefPtr gui) + + : mConfig(config), + camCalib(mConfig->calib.cameras[0]), + trackerNum(mConfig->trackerNum) + { + mPlayspace.Set(mConfig->manualCalib.GetAsReal()); + } + + //the way image is passed should change, probably no use having 3 args for it + void Update(RefPtr frame, + RefPtr workImg, + RefPtr drawImg, + RefPtr dets, + RefPtr> trackerUnits) + { + const double frameTimeAfterDetect = duration_cast(utils::SteadyTimer::Now() - frame->timestamp).count(); + + for (int index = 0; index < trackerUnits->size(); ++index) + { + auto& unit = (*trackerUnits)[index]; + // estimate the pose of current board + // NOTE: Pose from driver is already in local space, but scale must be aplied again. Pose should either be completely in local space or completely in global space + RodrPose scaledPoseFromDriver; + if (unit.WasVisibleToDriverLastFrame()) + scaledPoseFromDriver = RodrPose{unit.GetPoseFromDriver().position / mPlayspace.GetScale(), unit.GetPoseFromDriver().rotation}; + else + scaledPoseFromDriver = RodrPose{unit.GetEstimatedPose().position / mPlayspace.GetScale(), unit.GetEstimatedPose().rotation}; //if we dont have information from driver, use last local pose + // on rare occasions, detection crashes. Should be very rare and indicate something wrong with camera or tracker calibration + auto [estimatedPose, numEstimated] = math::EstimatePoseTracker( + dets->corners, dets->ids, unit.GetArucoBoard(), *camCalib, + unit.WasVisibleLastFrame() && mConfig->usePredictive, + scaledPoseFromDriver); + + estimatedPose.position *= mPlayspace.GetScale(); // unscale returned estimation; + unit.SetEstimatedPose(estimatedPose); + + ATT_ASSERT(!std::isnan(estimatedPose.position[X])); + + if (numEstimated <= 0) + { + unit.SetWasVisibleLastFrame(false); + continue; + } + unit.SetWasVisibleLastFrame(true); + + if (mConfig->depthSmoothing > 0 && unit.WasVisibleToDriverLastFrame() && !trackerCtrl->manualRecalibrate) + { + // depth estimation is noisy, so try to smooth it more, especialy if using multiple cameras + // if position is close to the position predicted by the driver, take the depth of the driver. + // if error is big, take the calculated depth + // error threshold is defined in the params as depth smoothing + RodrPose pose = unit.GetEstimatedPose(); + + const double distDriver = Length(unit.GetPoseFromDriver().position); + const double distPredict = Length(pose.position); + + const cv::Vec3d normPredict = pose.position / distPredict; + + double dist = std::abs(distPredict - distDriver); + dist = (dist / static_cast(mConfig->depthSmoothing)) + 0.1; + dist = std::clamp(dist, 0.0, 1.0); + + const double distFinal = (dist * distPredict) + (1 - dist) * distDriver; + + pose.position = normPredict * distFinal; + unit.SetEstimatedPose(pose); + } + + //clean up detections: if detection is outside of what camera can see (such as behind camera), it is deemed invalid and discarded + { + const cv::Point3d position = unit.GetEstimatedPose().position; + + // Reject detected positions that are behind the camera + if (position.z < 0) + { + unit.SetWasVisibleLastFrame(false); + continue; + } + + // Figure out the camera aspect ratio, XZ and YZ ratio limits + const double aspectRatio = GetMatSize(*workImg).aspectRatio(); + const double xzRatioLimit = 0.5 * static_cast(workImg->cols) / camCalib->cameraMatrix.at(0, 0); + const double yzRatioLimit = 0.5 * static_cast(workImg->rows) / camCalib->cameraMatrix.at(1, 1); + + // Figure out whether X or Y dimension is most likely to go outside the camera field of view + if (std::abs(position.x / position.y) > aspectRatio) + { + // Reject detections when XZ coordinate ratio goes out of camera FOV + if (std::abs(position.x / position.z) > xzRatioLimit) + { + unit.SetWasVisibleLastFrame(false); + continue; + } + } + else + { + // Reject detections when YZ coordinate ratio goes out of camera FOV + if (std::abs(position.y / position.z) > yzRatioLimit) + { + unit.SetWasVisibleLastFrame(false); + continue; + } + } + } + } + } +}; + +class Detect +{ +private: + + AprilTagWrapper april; + +public: + //merge this class with ApriltagWrapper? + explicit Detect(RefPtr config, + RefPtr vrDriver, + RefPtr gui) + + : april(AprilTagWrapper::ConvertFamily(config->markerLibrary), config->videoStreams[0]->quadDecimate, config->apriltagThreadCount) + {} + + //the way image is passed should change, probably no use having 3 args for it + void Update(RefPtr frame, + RefPtr workImg, + RefPtr drawImg, + RefPtr dets, + RefPtr> trackerUnits) + { + april.DetectMarkers(*workImg, *dets); + } +}; + +class Preprocess +{ +private: + utils::SteadyTimer detectionTimer{}; + int framesSinceLastSeen = 0; + static constexpr int framesToCheckAll = 20; + cv::Mat maskSearchImg{}; + cv::Mat tempGrayMaskedImg{}; + + RefPtr mConfig; + RefPtr camCalib; + RefPtr videoStream; + AprilTagWrapper april; + Index trackerNum; + RefPtr mPlayspace; + RefPtr mVRDriver; + RefPtr gui; + RefPtr> trackerUnits; + RefPtr vrClient; + RefPtr trackerCtrl; + +public: + explicit Preprocess(RefPtr config, + RefPtr vrDriver, + RefPtr gui) + + : mConfig(config), + camCalib(mConfig->calib.cameras[0]), + videoStream(mConfig->videoStreams[0]), + april(AprilTagWrapper::ConvertFamily(mConfig->markerLibrary), videoStream->quadDecimate, mConfig->apriltagThreadCount), + trackerNum(mConfig->trackerNum), + mVRDriver(vrDriver), + gui(gui) + + {} + + //the way image is passed should change, probably no use having 3 args for it + void Update(RefPtr frame, + RefPtr workImg, + RefPtr drawImg, + RefPtr dets, + RefPtr> trackerUnits) + { + + AprilTagWrapper::ConvertGrayscale(*workImg, *workImg); + const bool previewIsVisible = gui->IsPreviewVisible(); + + const auto stampBeforeDetect = utils::SteadyTimer::Now(); + detectionTimer.Restart(stampBeforeDetect); + + bool circularWindow = videoStream->circularWindow; + + // if any tracker was lost for longer than 20 frames, mark circularWindow as false + for (const auto& unit : *trackerUnits) + { + if (!unit.WasVisibleLastFrame()) + { + ++framesSinceLastSeen; + if (framesSinceLastSeen > framesToCheckAll) circularWindow = false; + break; + } + } + if (!circularWindow) framesSinceLastSeen = 0; + + // define our mask image. We want to create an image where everything but circles around predicted tracker positions will be black to speed up detection. + //tempGrayMaskedImg is also reset to black, or background will be old frames instead of pure black + if (GetMatSize(maskSearchImg) != GetMatSize(*workImg)) + { + maskSearchImg.create(GetMatSize(*workImg), CV_8U); + tempGrayMaskedImg.create(GetMatSize(*workImg), CV_8U); + } + tempGrayMaskedImg = cv::Scalar(0); + maskSearchImg = cv::Scalar(0); // fill with empty pixels + const int searchRadius = static_cast(static_cast(maskSearchImg.rows) * videoStream->searchWindow); + bool atleastOneTrackerVisible = false; + + const double frameTimeBeforeDetect = duration_cast(stampBeforeDetect - frame->timestamp).count(); + int trackerNum = trackerUnits->size(); + for (int i = 0; i < trackerNum; i++) + { + auto& unit = (*trackerUnits)[i]; + + std::array projected; + { + const cv::Vec3d unusedRVec{}; // used to perform change of basis + const cv::Vec3d unusedTVec{}; + const std::array points{unit.GetPoseFromDriver().position, unit.GetEstimatedPose().position}; + cv::projectPoints(points, unusedRVec, unusedTVec, camCalib->cameraMatrix, camCalib->distortionCoeffs, projected); + } + const auto& [driverCenter, previousCenter] = projected; + + // project point from position of tracker in camera 3d space to 2d camera pixel space, and draw a dot there + if (previewIsVisible) cv::circle(*drawImg, driverCenter, 5, cv::Scalar(0, 0, 255), 2, 8, 0); + + cv::Point2d maskCenter{-1,-1}; + + //if unit was seen on driver but not on camera, we use the position from driver for masking. If unit was seen on camera last frame, we always use that information for masking. + //if neither, maskCenter is -1,-1 and deemed invalid in next check + if (unit.WasVisibleToDriverLastFrame()) + { + //if (previewIsVisible) cv::drawFrameAxes(*drawImg, camCalib->cameraMatrix, camCalib->distortionCoeffs, unit.GetPoseFromDriver().rotation, math::ToVec(unit.GetPoseFromDriver().position), 0.10F); + + if (!unit.WasVisibleLastFrame()) + { + maskCenter = driverCenter; + } + else + { + maskCenter = previousCenter; + } + } + else + { + if (unit.WasVisibleLastFrame()) + { + maskCenter = previousCenter; + } + } + + //only draw if unit is visible on camera + if (maskCenter.inside(cv::Rect2d(0, 0, workImg->cols, workImg->rows))) + { + atleastOneTrackerVisible = true; + if (circularWindow) // if circular window is set mask a circle around the predicted tracker point + { + cv::circle(maskSearchImg, maskCenter, searchRadius, cv::Scalar(255), -1, 8, 0); + if (previewIsVisible) cv::circle(*drawImg, maskCenter, searchRadius, COLOR_MASK, 2, 8, 0); + } + else // if not, mask a vertical strip top to bottom. This happens every 20 frames if a tracker is lost. + { + const int maskX = static_cast(maskCenter.x); + const cv::Rect2i maskRect{cv::Point(maskX - searchRadius, 0), cv::Point2i(maskX + searchRadius, workImg->rows)}; + cv::rectangle(maskSearchImg, maskRect, cv::Scalar(255), -1); + if (previewIsVisible) cv::rectangle(*drawImg, maskRect, COLOR_MASK, 3); + } + } + else + { + unit.SetWasVisibleLastFrame(false); // if detected tracker is out of view of the camera, we mark it as not found, as either the prediction is wrong or we wont see it anyway + } + } + + // using copyTo with masking creates the image where everything but the locations where trackers are predicted to be is black + if (atleastOneTrackerVisible) + { + tempGrayMaskedImg.copyTo(*workImg, ~maskSearchImg); + } + } +}; class MainLoopRunner { @@ -80,7 +556,7 @@ class MainLoopRunner auto& unit = (*trackerUnits)[i]; auto [pose, isValid] = mVRDriver->GetTracker(i, -frameTimeBeforeDetect - videoStream->latency); - if(isValid) + if (isValid) pose = mPlayspace->InvTransformFromOVR(pose); std::array projected; diff --git a/AprilTagTrackers/tracker/PlayspaceCalib.hpp b/AprilTagTrackers/tracker/PlayspaceCalib.hpp index 20bb9cd5..0ab36fb8 100644 --- a/AprilTagTrackers/tracker/PlayspaceCalib.hpp +++ b/AprilTagTrackers/tracker/PlayspaceCalib.hpp @@ -28,6 +28,7 @@ class PlayspaceCalib Set(calib.posOffset, calib.angleOffset, calib.scale); } + //NOTE: should also apply scale? Pose Transform(const Pose& pose) const { return {mTransform * pose.position, mRotation * pose.rotation}; diff --git a/AprilTagTrackers/tracker/VRDriver.hpp b/AprilTagTrackers/tracker/VRDriver.hpp index 23a08530..121d88bc 100644 --- a/AprilTagTrackers/tracker/VRDriver.hpp +++ b/AprilTagTrackers/tracker/VRDriver.hpp @@ -16,27 +16,29 @@ namespace tracker class VRDriver { public: + VRDriver(){}; explicit VRDriver(const cfg::List& trackers); - void UpdateStation(Pose pose) { CmdUpdateStation(0, pose); } + virtual void UpdateStation(Pose pose) { CmdUpdateStation(0, pose); } // pose = x y z qw qx qy qz // 'updatepose' id pose time smoothing -> 'updated' - void UpdateTracker(int id, Pose pose, double frameTime, double smoothing); + virtual void UpdateTracker(int id, Pose pose, double frameTime, double smoothing); // 'settings' saved factor additional -> 'changed' - void SetSmoothing(double factor, double additional); + virtual void SetSmoothing(double factor, double additional); struct GetTrackerResult { Pose pose; bool isValid = false; }; + // 'gettrackerpose' id offset -> 'trackerpose' id pose status // status: -1 = invalid, 0 = valid, 1 = late - GetTrackerResult GetTracker(int id, double timeOffset); + virtual GetTrackerResult GetTracker(int id, double timeOffset); private: - void AddTracker(int id, cfg::TrackerRole role) + virtual void AddTracker(int id, cfg::TrackerRole role) { const std::string name = "ApriltagTracker" + std::to_string(id); std::string roleStr = "TrackerRole_"; @@ -45,13 +47,47 @@ class VRDriver } // 'updatestation' id pose -> 'updated' - void CmdUpdateStation(int id, Pose pose); + virtual void CmdUpdateStation(int id, Pose pose); + // 'numtrackers' -> 'numtrackers' count version + virtual int CmdGetTrackerCount(); + // 'addtracker' name role -> 'added' + virtual void CmdAddTracker(std::string_view name, std::string_view role); + // 'addstation' -> 'added' + virtual void CmdAddStation(); + + std::unique_ptr mBridge; +}; + +class DummyDriver : public VRDriver +{ +public: + explicit DummyDriver(const cfg::List& trackers) + {} + + void UpdateStation(Pose pose) {} + + // pose = x y z qw qx qy qz + // 'updatepose' id pose time smoothing -> 'updated' + void UpdateTracker(int id, Pose pose, double frameTime, double smoothing){}; + // 'settings' saved factor additional -> 'changed' + void SetSmoothing(double factor, double additional){}; + + // 'gettrackerpose' id offset -> 'trackerpose' id pose status + // status: -1 = invalid, 0 = valid, 1 = late + GetTrackerResult GetTracker(int id, double timeOffset){ return GetTrackerResult{Pose::Ident(), false}; }; + +private: + void AddTracker(int id, cfg::TrackerRole role) + {} + + // 'updatestation' id pose -> 'updated' + void CmdUpdateStation(int id, Pose pose){}; // 'numtrackers' -> 'numtrackers' count version - int CmdGetTrackerCount(); + int CmdGetTrackerCount() { return 3; }; // 'addtracker' name role -> 'added' - void CmdAddTracker(std::string_view name, std::string_view role); + void CmdAddTracker(std::string_view name, std::string_view role){}; // 'addstation' -> 'added' - void CmdAddStation(); + void CmdAddStation(){}; std::unique_ptr mBridge; };