void MultiTarget::update(ARMarkerInfo* targetInfo, int targetCount) { if (_active == false) { // If the target isn't active, then it can't be valid, and should not be updated either. _valid = false; return; } // Sanity check if (!targetInfo) { _valid = false; return; } _valid = (arMultiGetTransMat(targetInfo, targetCount, m_multi) >= 0); if (_valid) { double modelView[16]; arglCameraViewRH(m_multi->trans, modelView, 1.0f); updateTransform(osg::Matrix(modelView)); } }
/* main loop */ static void mainLoop(void) { ARUint8 *dataPtr; ARMarkerInfo *marker_info; int marker_num; float curPaddlePos[3]; int i; double err; /* grab a video frame */ if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) { arUtilSleep(2); return; } if( count == 0 ) arUtilTimerReset(); count++; /* detect the markers in the video frame */ if( arDetectMarkerLite(dataPtr, thresh, &marker_info, &marker_num) < 0 ) { cleanup(); exit(0); } argDrawMode2D(); if( !arDebug ) { argDispImage( dataPtr, 0,0 ); } else { argDispImage( dataPtr, 1, 1 ); if( arImageProcMode == AR_IMAGE_PROC_IN_HALF ) argDispHalfImage( arImage, 0, 0 ); else argDispImage( arImage, 0, 0); glColor3f( 1.0, 0.0, 0.0 ); glLineWidth( 1.0 ); for( i = 0; i < marker_num; i++ ) { argDrawSquare( marker_info[i].vertex, 0, 0 ); } glLineWidth( 1.0 ); } arVideoCapNext(); for( i = 0; i < marker_num; i++ ) marker_flag[i] = 0; /* get the paddle position */ paddleGetTrans(paddleInfo, marker_info, marker_flag, marker_num, &cparam); /* draw the 3D models */ glClearDepth( 1.0 ); glClear(GL_DEPTH_BUFFER_BIT); /* draw the paddle, base and menu */ if( paddleInfo->active ){ draw_paddle( paddleInfo); } /* get the translation from the multimarker pattern */ if( (err=arMultiGetTransMat(marker_info, marker_num, config)) < 0 ) { argSwapBuffers(); return; } //printf("err = %f\n", err); if(err > 100.0 ) { argSwapBuffers(); return; } //draw a red ground grid drawGroundGrid( config->trans, 20, 150.0f, 105.0f, 0.0f); /* find the paddle position relative to the base */ findPaddlePosition(curPaddlePos, paddleInfo->trans, config->trans); /* check for collisions with targets */ for(i=0;i<TARGET_NUM;i++){ myTarget[i].state = NOT_TOUCHED; if(checkCollision(curPaddlePos, myTarget[i].pos, 20.0f)) { myTarget[i].state = TOUCHED; fprintf(stderr,"touched !!\n"); } } /* draw the targets */ for(i=0;i<TARGET_NUM;i++){ draw(myTarget[i],config->trans); } argSwapBuffers(); }
/* main loop */ static void mainLoop(void) { ARUint8 *dataPtr; ARMarkerInfo *marker_info; int marker_num; float curPaddlePos[3]; int i; double err; double angle; err=0.; /* grab a video frame */ if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) { arUtilSleep(2); return; } if( count == 0 ) arUtilTimerReset(); count++; /* detect the markers in the video frame */ if( arDetectMarkerLite(dataPtr, thresh, &marker_info, &marker_num) < 0 ) { cleanup(); exit(0); } argDrawMode2D(); if( !arDebug ) { argDispImage( dataPtr, 0,0 ); } else { argDispImage( dataPtr, 1, 1 ); if( arImageProcMode == AR_IMAGE_PROC_IN_HALF ) argDispHalfImage( arImage, 0, 0 ); else argDispImage( arImage, 0, 0); glColor3f( 1.0, 0.0, 0.0 ); glLineWidth( 1.0 ); for( i = 0; i < marker_num; i++ ) { argDrawSquare( marker_info[i].vertex, 0, 0 ); } glLineWidth( 1.0 ); } arVideoCapNext(); for( i = 0; i < marker_num; i++ ) marker_flag[i] = 0; /* get the paddle position */ paddleGetTrans(paddleInfo, marker_info, marker_flag, marker_num, &cparam); /* draw the 3D models */ glClearDepth( 1.0 ); glClear(GL_DEPTH_BUFFER_BIT); /* get the translation from the multimarker pattern */ if( (err=arMultiGetTransMat(marker_info, marker_num, config)) < 0 ) { argSwapBuffers(); return; } // printf("err = %f\n", err); if(err > 100.0 ) { argSwapBuffers(); return; } //draw a red ground grid drawGroundGrid( config->trans, 15, 150.0, 110.0, 0.0); /* find the paddle position relative to the base */ if (paddleInfo->active) findPaddlePosition(curPaddlePos,paddleInfo->trans,config->trans); /* checking for paddle gesture */ if( paddleInfo->active) { int findItem=-1; if (myPaddleItem.item!=-1) { if( check_incline(paddleInfo->trans, config->trans, &angle) ) { myPaddleItem.x += 2.0 * cos(angle); myPaddleItem.y += 2.0 * sin(angle); if( myPaddleItem.x*myPaddleItem.x + myPaddleItem.y*myPaddleItem.y > 900.0 ) { myPaddleItem.x -= 2.0 * cos(angle); myPaddleItem.y -= 2.0 * sin(angle); myListItem.item[myPaddleItem.item].onpaddle=0; myListItem.item[myPaddleItem.item].pos[0]=curPaddlePos[0]; myListItem.item[myPaddleItem.item].pos[1]=curPaddlePos[1]; myPaddleItem.item = -1; } } } else { if ((findItem=check_pickup(paddleInfo->trans, config->trans,&myListItem, &angle))!=-1) { myPaddleItem.item=findItem; myPaddleItem.x =0.0; myPaddleItem.y =0.0; myPaddleItem.angle = 0.0; myListItem.item[myPaddleItem.item].onpaddle=1; } } } /* draw the item */ drawItems(config->trans,&myListItem); /* draw the paddle */ if( paddleInfo->active ){ draw_paddle(paddleInfo,&myPaddleItem); } argSwapBuffers(); }
/* main loop */ static void mainLoop(void) { ARUint8 *dataPtr; ARMarkerInfo *marker_info; int marker_num; double err; int i; /* grab a vide frame */ if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) { arUtilSleep(2); return; } if( count == 0 ) arUtilTimerReset(); count++; /* detect the markers in the video frame */ if( arDetectMarkerLite(dataPtr, thresh, &marker_info, &marker_num) < 0 ) { cleanup(); exit(0); } argDrawMode2D(); if( !arDebug ) { argDispImage( dataPtr, 0,0 ); } else { argDispImage( dataPtr, 1, 1 ); if( arImageProcMode == AR_IMAGE_PROC_IN_HALF ) argDispHalfImage( arImage, 0, 0 ); else argDispImage( arImage, 0, 0); glColor3f( 1.0, 0.0, 0.0 ); glLineWidth( 1.0 ); for( i = 0; i < marker_num; i++ ) { argDrawSquare( marker_info[i].vertex, 0, 0 ); } glLineWidth( 1.0 ); } arVideoCapNext(); if( (err=arMultiGetTransMat(marker_info, marker_num, config)) < 0 ) { argSwapBuffers(); return; } printf("err = %f\n", err); if(err > 100.0 ) { argSwapBuffers(); return; } /* for(i=0;i<3;i++) { for(j=0;j<4;j++) printf("%10.5f ", config->trans[i][j]); printf("\n"); } printf("\n"); */ argDrawMode3D(); argDraw3dCamera( 0, 0 ); glClearDepth( 1.0 ); glClear(GL_DEPTH_BUFFER_BIT); for( i = 0; i < config->marker_num; i++ ) { if( config->marker[i].visible >= 0 ) draw( config->trans, config->marker[i].trans, 0 ); else draw( config->trans, config->marker[i].trans, 1 ); } argSwapBuffers(); }
/* main loop */ static void mainLoop(void) { ARUint8 *dataPtr; ARMarkerInfo *marker_info; int marker_num; double err; int i; /* grab a vide frame */ if( (dataPtr = (ARUint8 *)arVideoGetImage()) == NULL ) { arUtilSleep(2); return; } if( count == 0 ) arUtilTimerReset(); count++; /* detect the markers in the video frame */ if( arDetectMarkerLite(dataPtr, thresh, &marker_info, &marker_num) < 0 ) { cleanup(); exit(0); } argDrawMode2D(); if( !arDebug ) { argDispImage( dataPtr, 0,0 ); } else { argDispImage( dataPtr, 1, 1 ); if( arImageProcMode == AR_IMAGE_PROC_IN_HALF ) argDispHalfImage( arImage, 0, 0 ); else argDispImage( arImage, 0, 0); glColor3f( 1.0, 0.0, 0.0 ); glLineWidth( 1.0 ); for( i = 0; i < marker_num; i++ ) { argDrawSquare( marker_info[i].vertex, 0, 0 ); } glLineWidth( 1.0 ); } arVideoCapNext(); if( (err=arMultiGetTransMat(marker_info, marker_num, config)) < 0 ) { argSwapBuffers(); return; } printf("err = %f\n", err); if(err > 100.0 ) { argSwapBuffers(); return; } /* for(i=0;i<3;i++) { for(j=0;j<4;j++) printf("%10.5f ", config->trans[i][j]); printf("\n"); } printf("\n"); */ argDrawMode3D(); argDraw3dCamera( 0, 0 ); glClearDepth( 1.0 ); glScalef(1.0,1.0,5.0); glClear(GL_DEPTH_BUFFER_BIT); //PRINT DOS PREDIOS FANTASMAS (COM E SEM MARCADORES) if (mostraFantasmas == 1) { desenhaFantasmasSemTag(); //Desenha predios fantasmas com marcadores identificados glColorMask(GL_FALSE, GL_FALSE, GL_FALSE, GL_FALSE); for(i = (config->marker_num) - 3; i < config->marker_num; i++ ) { if( config->marker[i].visible >= 0 ) { glScalef(1.0,1.0,2.0); draw( config->trans, config->marker[i].trans, 0 ); glScalef(1.0,1.0,0.5); }else{ glScalef(1.0,1.0,2.0); draw( config->trans, config->marker[i].trans, 1 ); glScalef(1.0,1.0,0.5); } } glColorMask(GL_TRUE, GL_TRUE, GL_TRUE, GL_TRUE); } desenhaCarros(); for (i = 0; i < (config->marker_num) - 3; i++ ) { if( config->marker[i].visible >= 0 ) draw( config->trans, config->marker[i].trans, 0 ); else draw( config->trans, config->marker[i].trans, 1 ); } argSwapBuffers(); }
void ARMultiPublisher::getTransformationCallback(const sensor_msgs::ImageConstPtr & image_msg) { // Get the image from ROSTOPIC // NOTE: the dataPtr format is BGR because the ARToolKit library was // build with V4L, dataPtr format change according to the // ARToolKit configure option (see config.h). try { capture_ = bridge_.imgMsgToCv(image_msg, "bgr8"); } catch (sensor_msgs::CvBridgeException & e) { ROS_ERROR ("Could not convert from >%s< to 'bgr8'.", image_msg->encoding.c_str ()); } // cvConvertImage(capture,capture,CV_CVTIMG_FLIP); ARUint8* data_ptr = (ARUint8 *)capture_->imageData; // detect the markers in the video frame if (arDetectMarker(data_ptr, threshold_, &marker_info_, &num_detected_marker_) < 0) { argCleanup(); ROS_BREAK (); } ROS_DEBUG("Detected >%i< of >%i< markers.", num_detected_marker_, num_total_markers_); double error = 0.0; if ((error = arMultiGetTransMat(marker_info_, num_detected_marker_, multi_marker_config_)) < 0) { // ROS_ERROR("Could not get transformation. This should never happen."); ROS_WARN("Could not get transformation."); return; } ROS_DEBUG("Error is >%f<.", error); for (int i = 0; i < num_detected_marker_; i++) { ROS_DEBUG("multi_marker_config_->prevF: %i", multi_marker_config_->prevF); ROS_DEBUG("%s: (%i) pos: %f %f id: %i cf: %f", marker_frame_.c_str(), i, marker_info_[i].pos[0], marker_info_[i].pos[1], marker_info_[i].id, marker_info_[i].cf); } // choose those with the highest confidence std::vector<double> cfs(num_total_markers_, 0.0); marker_indizes_.clear(); for (int i = 0; i < num_total_markers_; ++i) { marker_indizes_.push_back(-1); } for (int i = 0; i < num_total_markers_; ++i) { for (int j = 0; j < num_detected_marker_; j++) { if (!(marker_info_[j].id < 0)) { if (marker_info_[j].cf > cfs[marker_info_[j].id]) { cfs[marker_info_[j].id] = marker_info_[j].cf; marker_indizes_[marker_info_[j].id] = j; } } } } double ar_quat[4], ar_pos[3]; arUtilMat2QuatPos(multi_marker_config_->trans, ar_quat, ar_pos); tf::Quaternion rotation(-ar_quat[0], -ar_quat[1], -ar_quat[2], ar_quat[3]); tf::Vector3 origin(ar_pos[0] * AR_TO_ROS, ar_pos[1] * AR_TO_ROS, ar_pos[2] * AR_TO_ROS); tf::Transform transform(rotation, origin); if (multi_marker_config_->prevF && publish_tf_) { if(error < error_threshold_) { ROS_DEBUG("%f %f %f | %f %f %f %f | %f", origin.getX(), origin.getY(), origin.getZ(), rotation.getX(), rotation.getY(), rotation.getZ(), rotation.getW(), image_msg->header.stamp.toSec()); tf::StampedTransform cam_to_marker(transform, image_msg->header.stamp, camera_frame_, marker_frame_); tf_broadcaster_.sendTransform(cam_to_marker); } publishErrorMarker(error, image_msg->header.stamp); } if(publish_visual_markers_) { for (int i = 0; i < num_total_markers_; i++) { if (marker_indizes_[i] >= 0) { tf::Transform marker_transform; getTransform(i, marker_transform); tf::Transform marker = transform * marker_transform; publishMarker(i, marker, image_msg->header.stamp); last_transform_ = marker; } // else // { // publishMarker(i, last_transform_, image_msg->header.stamp); // } } } }