quint64 NASM::getMainOffset(QFile &lst, QString entryLabel)
{
QTextStream lstStream(&lst);
QRegExp mainLabel(entryLabel + ":");
mainLabel.setCaseSensitivity(Qt::CaseInsensitive);
bool flag = false;
while (!lstStream.atEnd()) {
QString line = lstStream.readLine();
if (flag) {
//! Omit strings with data only
//! if in list : line number, address, data and it is all (without instruction) -
//! omit this string
if (line.length() <= 37) {
continue;
}
QByteArray lineArr = line.toLocal8Bit();
const char *s = lineArr.constData();
quint64 a, b, c;
if (sscanf(s, "%llu %llx %llx", &a, &b, &c) == 3) {
//! Exclude 0 0
if (!(b == 0 && c == 0)) {
return b;
}
}
} else {
if (line.indexOf(mainLabel) != -1)
flag = true;
}
}
return -1;
}
void MultiCursorAppCpp::detectHeadPosition(CvBlobs blobs)
{
// Initialize userData
userData.clear();
// 前フレームのラベルを見て最も投票数の多かったラベルを前フレームの対応領域とする
for (CvBlobs::const_iterator it = blobs.begin(); it != blobs.end(); ++it)
{
if (!preLabelMat.empty())
{
// 前フレームのラベルを投票
vector<Point2i> checkLabel;
checkLabel.push_back(Point2i(0, 0)); // init checkLabel;
for (int y = it->second->miny; y < it->second->maxy; ++y)
{
for (int x = it->second->minx; x < it->second->maxx; ++x)
{
unsigned long preLabel = preLabelMat.at<unsigned long>(y, x);
if (it->first == labelMat.at<unsigned long>(y, x) && preLabel != 0)
{
bool isFoundLabel = false;
for (int i = 0; i < checkLabel.size(); ++i)
{
if (checkLabel[i].x == preLabel)
{
++checkLabel[i].y;
isFoundLabel = true;
break;
}
}
if (!isFoundLabel)
{
Point2i newPoint(preLabel, 1); // (labelID, # of label)
checkLabel.push_back(newPoint);
}
}
}
}
// 最も投票数の多いラベルを探す
Point2i mainLabel(0, 0);
for (int i = 0; i < checkLabel.size(); ++i)
{
if (checkLabel[i].y > mainLabel.y)
{
mainLabel.x = checkLabel[i].x;
mainLabel.y = checkLabel[i].y;
}
}
// 前フレームのデータと対応付けて現フレームのデータを作る
UserData newUserData;
newUserData.isDataFound = false;
if (mainLabel.y > it->second->area / 2) // 半分以上を占める領域がないときは無視
{
for (vector<UserData>::iterator p = preUserData.begin(); p != preUserData.end(); ++p)
{
if (p->labelID == mainLabel.x)
{
p->isDataFound = true;
newUserData.isDataFound = true;
newUserData.headInfo.height = p->headInfo.height;
newUserData.headInfo.depthPoint.x = p->headInfo.depthPoint.x;
newUserData.headInfo.depthPoint.y = p->headInfo.depthPoint.y;
break;
}
}
}
newUserData.labelID = it->first;
newUserData.centroid.x = it->second->centroid.x;
newUserData.centroid.y = it->second->centroid.y;
userData.push_back(newUserData);
}
else
{
UserData newUserData;
newUserData.isDataFound = false;
newUserData.labelID = it->first;
newUserData.centroid.x = it->second->centroid.x;
newUserData.centroid.y = it->second->centroid.y;
userData.push_back(newUserData);
}
}
// Update preLabel
preLabelMat = labelMat;
// Find the highest point of each user area
USHORT* headHeights = new USHORT[blobs.size()];
Point2i* newHighestPositions = new Point2i[blobs.size()];
int blobID = 0;
for (CvBlobs::const_iterator it = blobs.begin(); it != blobs.end(); it++)
{
headHeights[blobID] = 0;
newHighestPositions[blobID].x = 0;
newHighestPositions[blobID].y = 0;
for (int y = it->second->miny; y <= it->second->maxy; y++)
{
for (int x = it->second->minx; x <= it->second->maxx; x++)
{
if (0 <= blobID && blobID < blobs.size())
{
USHORT height = *heightMatrix.ptr<USHORT>(y, x);
if (headHeights[blobID] < height && height < HEAD_HEIGHT_MAX)
{
headHeights[blobID] = height;
newHighestPositions[blobID].x = x;
newHighestPositions[blobID].y = y;
}
}
}
}
// Debug: Show the highest point of each users
// circle(userAreaMat, Point(newHighestPositions[blobID].x, newHighestPositions[blobID].y), 5, Scalar(255, 0, 255), 3);
blobID++;
}
// Define users' head positions
Point2i* newHeadPositions = new Point2i[blobs.size()];
INT* numHeadPoints = new INT[blobs.size()];
blobID = 0;
for (CvBlobs::const_iterator it = blobs.begin(); it != blobs.end(); ++it)
{
// Set the highest position as a base point of searching
userData[blobID].headInfo.depthPoint.x = newHighestPositions[blobID].x;
userData[blobID].headInfo.depthPoint.y = newHighestPositions[blobID].y;
userData[blobID].headInfo.height = headHeights[blobID];
if (userData[blobID].isDataFound)
{
// Check distance between 2d positions in current frame and in preframe
float distance = sqrt(
pow(userData[blobID].headInfo.depthPoint.x - preUserData[blobID].headInfo.depthPoint.x, 2)
+ pow(userData[blobID].headInfo.depthPoint.y - preUserData[blobID].headInfo.depthPoint.y, 2)
);
float distanceZ = abs(heightMatrix.at<float>(userData[blobID].headInfo.depthPoint.y, userData[blobID].headInfo.depthPoint.x) - preUserData[blobID].headInfo.height);
//cout << distance << endl;
// If the point is far from predata, just use pre-data / もし前回のフレームより大きく頭の位置がずれていたら前回の値を使う
if (distance > 80.0f || distanceZ > 1000.0f)
{
userData[blobID].headInfo.height = preUserData[blobID].headInfo.height;
userData[blobID].headInfo.depthPoint.x = preUserData[blobID].headInfo.depthPoint.x;
userData[blobID].headInfo.depthPoint.y = preUserData[blobID].headInfo.depthPoint.y;
}
}
// Estimate exact head positions (Get average)
numHeadPoints[blobID] = 0;
newHeadPositions[blobID].x = 0;
newHeadPositions[blobID].y = 0;
#if 1
int offset_head = 40; // ミーティングルーム用
#else
int offset_head = 100; // 作業場用
#endif
int minY = userData[blobID].headInfo.depthPoint.y - offset_head; if (minY < 0) minY = 0;
int maxY = userData[blobID].headInfo.depthPoint.y + offset_head; if (maxY > kinectBasics.heightDepth) maxY = kinectBasics.heightDepth;
int minX = userData[blobID].headInfo.depthPoint.x - offset_head; if (minX < 0) minX = 0;
int maxX = userData[blobID].headInfo.depthPoint.x + offset_head; if (maxX > kinectBasics.widthDepth) maxX = kinectBasics.widthDepth;
for (int y = minY; y <= maxY; y++)
{
for (int x = minX; x <= maxX; x++)
{
USHORT height = *heightMatrix.ptr<USHORT>(y, x);
//cout << it->first << ", " << labelMat.at<unsigned long>(y, x) << endl;
if ((userData[blobID].headInfo.height - HEAD_LENGTH) < height && height < HEAD_HEIGHT_MAX
&& it->first == labelMat.at<unsigned long>(y, x) // 同じblob内のみ探索
)
{
newHeadPositions[blobID].x += x;
newHeadPositions[blobID].y += y;
numHeadPoints[blobID]++;
}
}
}
blobID++;
}
// Make avarage pixel value of each head positions the head positions of users
for (int i = 0; i < blobs.size(); i++)
{
if (numHeadPoints[i] != 0)
{
// Calculate head position in 2D pixel
userData[i].headInfo.depthPoint.x = newHeadPositions[i].x / numHeadPoints[i];
userData[i].headInfo.depthPoint.y = newHeadPositions[i].y / numHeadPoints[i];
}
else
{
// 点が見つからなかった場合は最も高い点を頭にする
userData[i].headInfo.depthPoint.x = newHighestPositions[i].x;
userData[i].headInfo.depthPoint.y = newHighestPositions[i].y;
}
// Calculate head position in 3D point
float* headPosition = point3fMatrix.ptr<float>(userData[i].headInfo.depthPoint.y, userData[i].headInfo.depthPoint.x);
userData[i].headInfo.cameraPoint.x = headPosition[0];
userData[i].headInfo.cameraPoint.y = headPosition[1];
userData[i].headInfo.cameraPoint.z = headPosition[2] + 0.2;
// Debug: Show the head point
circle(userAreaMat, Point(userData[i].headInfo.depthPoint.x, userData[i].headInfo.depthPoint.y), 7, Scalar(255, 0, 0), 3);
}
}
