Esempio n. 1
0
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);
	}
}