Trace OSVR 演算法 (VideoBasedTracker)
原始程式碼如下：
videobasedtracker/VideoBasedTracker.cpp
以下為簡化過程式碼的流程：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
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56bool VideoBasedTracker::processImage(cv::Mat frame, cv::Mat grayImage,
OSVR_TimeValue const &tv,
PoseHandler handler) {
m_frame = frame;
m_imageGray = grayImage;
auto foundLeds = m_blobExtractor.extractBlobs(grayImage); // blob 偵測
auto undistortedLeds = undistortLeds(foundLeds, m_camParams);
for (size_t sensor = 0; sensor < m_identifiers.size(); sensor++) {
osvrPose3SetIdentity(&m_pose);
auto ledsMeasurements = undistortedLeds;
{
auto &myLeds = m_led_groups[sensor];
auto led = begin(myLeds);
auto e = end(myLeds);
while (led != end(myLeds)) {
led->resetUsed();
auto threshold = m_params.blobMoveThreshold *
led->getMeasurement().diameter;
auto nearest = led->nearest(ledsMeasurements, threshold); // 找最近的
if (nearest == end(ledsMeasurements)) {
// We have no blob corresponding to this LED, so we need
// to delete this LED.
led = myLeds.erase(led);
} else {
// Update the values in this LED and then go on to the
// next one. Remove this blob from the list of potential matches.
// 因為已經這個 blob 是哪個 LED, 所以從 ledsMeasurements 裡移除這個 LED
led->addMeasurement(*nearest, m_params.blobsKeepIdentity);
ledsMeasurements.erase(nearest);
++led;
}
} // while
for (auto &remainingLed : ledsMeasurements) {
myLeds.emplace_back(m_identifiers[sensor].get(), remainingLed);
}
}
bool gotPose = false;
if (m_estimators[sensor]) {
OSVR_PoseState pose;
if (m_estimators[sensor]->EstimatePoseFromLeds( // 估算 Pose
m_led_groups[sensor], tv, pose)) {
m_pose = pose;
handler(static_cast<unsigned>(sensor), pose); // 送 Pose 出去
gotPose = true;
}
}
} // for
return done;
}
ledsMeasurements 是偵測到的 LED blobs
重點流程如下：
- m_blobExtractor.extractBlobs(grayImage); // blob 偵測
- m_estimators[sensor]->EstimatePoseFromLeds() // 估算 Pose
- handler(static_cast
(sensor), pose); // 送 Pose 出去