Trace OSVR 演算法

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
56
bool 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 出去

相關程式碼:
videobasedtracker/LED.h
videobasedtracker/LED.cpp