Exemplo n.º 1
0
bool ARtagLocalizer::getARtagPose(IplImage* src, IplImage* dst, int camID)
{
    if (!init)
    {
        printf("Did not initalize the ARtagLocalizer!!\n");
        return NULL;
    }
    if (src->width != imgwidth || src->height != imgheight)
    {
        printf("src->width: %d src->height %d\n", src->width, src->height);
        printf("imgwidth: %d imgheight %d\n", imgwidth, imgheight);
        printf("Image passed in does not match initialized image size!!\n");
        return NULL;
    }
    if (src->nChannels != 1)
    {
        printf("Please pass in grayscale image into ARtagLocalizer! \n");
        return NULL;
    }

    int numMarkers = 0;
    ARToolKitPlus::ARMarkerInfo* markers = NULL;
    if (tracker->arDetectMarker(const_cast<unsigned char*>((unsigned char*)src->imageData), 150, &markers, &numMarkers) < 0)
    {
        return false;
    }

    mytag.clear();

    float modelViewMatrix_[16];
    for(int m = 0; m < numMarkers; ++m) {
        if(markers[m].id != -1 && markers[m].cf >= 0.5) {
            tracker->calcOpenGLMatrixFromMarker(&markers[m], patternCenter_, patternWidth_, modelViewMatrix_);

            float x = modelViewMatrix_[12] / 1000.0;
            float y = modelViewMatrix_[13] / 1000.0;
            float z = modelViewMatrix_[14] / 1000.0;
            float yaw = -atan2(modelViewMatrix_[1], modelViewMatrix_[0]);
            if (yaw < 0)
            {
                yaw += 6.28;
            }

            if ((x == 0.0 && y == 0.0 && yaw == 0.0) || (x > 10000.0 && y > 10000.0) || (x < -10000.0 && y < -10000.0) || (z <= 0.001))
            {
                // ARTKPlus bug that occurs sometimes
                continue;
            }

//            printf("Id: %d\t Conf: %.2f\n", markers[m].id, markers[m].cf);
//			printf("x: %.2f \t y: %.2f \t z: %.2f \t yaw: %.2f\n", x,y,z,yaw);
//                        printf("\n");

//            char str[30];
//            sprintf(str,"%d",markers[m].id);
//            cvPutText (dst,str,cvPoint( markers[m].pos[0]+25,markers[m].pos[1]+10),&cvFont(3,3),cvScalar(255,0,0));
//            sprintf(str,"(%.2f,%.2f,%.2f)", x*fudge + xoffset, -(y*fudge + yoffset), yaw + yawoffset);
//            cvPutText (dst,str,cvPoint( markers[m].pos[0]+25,markers[m].pos[1]+25),&cvFont(1,1),cvScalar(255,0,0));

            cv::Mat PoseM(4, 4, CV_32F, modelViewMatrix_);
            cv::transpose(PoseM,PoseM);
            CvMat pose = PoseM;

            // save artag struct for access later
            if (markers[m].id >= 0 && markers[m].id < 50 && !allStop)
            {
                //				EnterCriticalSection(&tags_mutex);
//                ARtag * ar = tags[markers[m].id];
//                ar->setId(markers[m].id);
//                ar->setPose(&pose);
//                ar->setPoseAge(0);
//                ar->setCamId(camID);
//                ar->setLocation(markers[m].pos[0], markers[m].pos[1]);

                ARtag mt;
                mt.setId(markers[m].id);
                mt.setPose(&pose);
                mt.setPoseAge(0);
                mt.setCamId(camID);
                mt.setLocation(markers[m].pos[0], markers[m].pos[1]);
                //				LeaveCriticalSection(&tags_mutex);
                mytag.push_back(mt);
            }
        }
    }

    return true;
}
bool getARtagPose(IplImage * src, IplImage *dst, vector<ARtag>& tags, int camNum)
{
	if (trackers[camNum] == NULL) {
		// This camera does not have a tracker object.
		return false;
	}
	if (!init)
	{
		printf("Did not initalize the ARtagLocalizer!!\n");
		return NULL;
	}
	if (src->width != imgwidth || src->height != imgheight)
	{
		printf("Image passed in does not match initialized image size!!\n");
		return NULL;
	}
	if (src->nChannels != 1)
	{
		printf("Please pass in grayscale image into ARtagLocalizer! \n");
		return NULL;
	}
	
	int n = 0;
	for(int i = 0; i < src->height; ++i)
		for(int j = 0; j < src->width; ++j)
			cameraBuffer[n++] = CV_IMAGE_ELEM(src,uchar,i,j);

	/*const char* description = tracker->getDescription();
	printf("ARToolKitPlus compile-time information:\n%s\n\n", description);*/

	int numMarkers = 0;
	ARToolKitPlus::ARMarkerInfo* markers = NULL;
	if (trackers[camNum]->arDetectMarker(const_cast<unsigned char*>(cameraBuffer), 150, &markers, &numMarkers) < 0) 
	{
		return false;
	}

	float modelViewMatrix_[16];

	for(int m = 0; m < numMarkers; ++m) {
		if(markers[m].id != -1 && markers[m].cf >= 0.5) {
			trackers[camNum]->calcOpenGLMatrixFromMarker(&markers[m], patternCenter_, patternWidth_, modelViewMatrix_);

			ARtag ar;
			ar.setId(markers[m].id);

			float x = modelViewMatrix_[12] / 1000.0;
			float y = modelViewMatrix_[13] / 1000.0;
			float z = modelViewMatrix_[14] / 1000.0;
			float yaw = -atan2(modelViewMatrix_[1], modelViewMatrix_[0]);

			if ((x == 0 && y == 0 && yaw == 0) || (x > 10000 && y > 10000) || (x < -10000 && y < -10000) || z <= 0.01)
			{
				// ARTKPlus bug that occurs sometimes
				continue;
			}			
			
			char str[30];
			sprintf(str,"%d",markers[m].id);
			cvPutText (dst,str,cvPoint( markers[m].pos[0]+25,markers[m].pos[1]+10),&cvFont(3,3),cvScalar(0,0,255));
			sprintf(str,"(%.2f,%.2f,%.2f)", x, y, yaw);
			cvPutText (dst,str,cvPoint( markers[m].pos[0]+25,markers[m].pos[1]+25),&cvFont(1,1),cvScalar(0,0,255));

			cv::Mat PoseM(4, 4, CV_32F, modelViewMatrix_);
			cv::transpose(PoseM,PoseM);
			CvMat pose = PoseM;
			ar.setPose(&pose);
			ar.setPoseAge(0);
			tags.push_back(ar);
		}
	}


	//google::protobuf::ShutdownProtobufLibrary();
	//printf("\n\n");

	return true;
}