void ARMultiPublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg) { ARUint8 *dataPtr; ARMarkerInfo *marker_info; int marker_num; int i, k, j; /* 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).*/ #if ROS_VERSION_MINIMUM(1, 9, 0) try { capture_ = cv_bridge::toCvCopy (image_msg, sensor_msgs::image_encodings::BGR8); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge exception: %s", e.what()); } dataPtr = (ARUint8 *) ((IplImage) capture_->image).imageData; #else try { capture_ = bridge_.imgMsgToCv (image_msg, "bgr8"); } catch (sensor_msgs::CvBridgeException & e) { ROS_ERROR("cv_bridge exception: %s", e.what()); } dataPtr = (ARUint8 *) capture_->imageData; #endif // detect the markers in the video frame if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0) { argCleanup (); ROS_BREAK (); } arPoseMarkers_.markers.clear (); // check for known patterns for (i = 0; i < objectnum; i++) { k = -1; for (j = 0; j < marker_num; j++) { if (object[i].id == marker_info[j].id) { if (k == -1) k = j; else // make sure you have the best pattern (highest confidence factor) if (marker_info[k].cf < marker_info[j].cf) k = j; } } if (k == -1) { object[i].visible = 0; continue; } // calculate the transform for each marker if (object[i].visible == 0) { arGetTransMat (&marker_info[k], object[i].marker_center, object[i].marker_width, object[i].trans); } else { arGetTransMatCont (&marker_info[k], object[i].trans, object[i].marker_center, object[i].marker_width, object[i].trans); } object[i].visible = 1; double arQuat[4], arPos[3]; //arUtilMatInv (object[i].trans, cam_trans); arUtilMat2QuatPos (object[i].trans, arQuat, arPos); // **** convert to ROS frame double quat[4], pos[3]; pos[0] = arPos[0] * AR_TO_ROS; pos[1] = arPos[1] * AR_TO_ROS; pos[2] = arPos[2] * AR_TO_ROS; quat[0] = -arQuat[0]; quat[1] = -arQuat[1]; quat[2] = -arQuat[2]; quat[3] = arQuat[3]; ROS_DEBUG (" Object num %i ------------------------------------------------", i); ROS_DEBUG (" QUAT: Pos x: %3.5f y: %3.5f z: %3.5f", pos[0], pos[1], pos[2]); ROS_DEBUG (" Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]); // **** publish the marker ar_pose::ARMarker ar_pose_marker; ar_pose_marker.header.frame_id = image_msg->header.frame_id; ar_pose_marker.header.stamp = image_msg->header.stamp; ar_pose_marker.id = object[i].id; ar_pose_marker.pose.pose.position.x = pos[0]; ar_pose_marker.pose.pose.position.y = pos[1]; ar_pose_marker.pose.pose.position.z = pos[2]; ar_pose_marker.pose.pose.orientation.x = quat[0]; ar_pose_marker.pose.pose.orientation.y = quat[1]; ar_pose_marker.pose.pose.orientation.z = quat[2]; ar_pose_marker.pose.pose.orientation.w = quat[3]; ar_pose_marker.confidence = round(marker_info->cf * 100); arPoseMarkers_.markers.push_back (ar_pose_marker); // **** publish transform between camera and marker #if ROS_VERSION_MINIMUM(1, 9, 0) tf::Quaternion rotation (quat[0], quat[1], quat[2], quat[3]); tf::Vector3 origin (pos[0], pos[1], pos[2]); tf::Transform t (rotation, origin); #else // DEPRECATED: Fuerte support ends when Hydro is released btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]); btVector3 origin (pos[0], pos[1], pos[2]); btTransform t (rotation, origin); #endif if (publishTf_) { tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, object[i].name); broadcaster_.sendTransform(camToMarker); } // **** publish visual marker if (publishVisualMarkers_) { #if ROS_VERSION_MINIMUM(1, 9, 0) tf::Vector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS); tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin); tf::Transform markerPose = t * m; // marker pose in the camera frame #else // DEPRECATED: Fuerte support ends when Hydro is released btVector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS); btTransform m (btQuaternion::getIdentity (), markerOrigin); btTransform markerPose = t * m; // marker pose in the camera frame #endif tf::poseTFToMsg (markerPose, rvizMarker_.pose); rvizMarker_.header.frame_id = image_msg->header.frame_id; rvizMarker_.header.stamp = image_msg->header.stamp; rvizMarker_.id = object[i].id; rvizMarker_.scale.x = 1.0 * object[i].marker_width * AR_TO_ROS; rvizMarker_.scale.y = 1.0 * object[i].marker_width * AR_TO_ROS; rvizMarker_.scale.z = 0.5 * object[i].marker_width * AR_TO_ROS; rvizMarker_.ns = "basic_shapes"; rvizMarker_.type = visualization_msgs::Marker::CUBE; rvizMarker_.action = visualization_msgs::Marker::ADD; switch (i) { case 0: rvizMarker_.color.r = 0.0f; rvizMarker_.color.g = 0.0f; rvizMarker_.color.b = 1.0f; rvizMarker_.color.a = 1.0; break; case 1: rvizMarker_.color.r = 1.0f; rvizMarker_.color.g = 0.0f; rvizMarker_.color.b = 0.0f; rvizMarker_.color.a = 1.0; break; default: rvizMarker_.color.r = 0.0f; rvizMarker_.color.g = 1.0f; rvizMarker_.color.b = 0.0f; rvizMarker_.color.a = 1.0; } rvizMarker_.lifetime = ros::Duration (1.0); rvizMarkerPub_.publish(rvizMarker_); ROS_DEBUG ("Published visual marker"); } } arMarkerPub_.publish(arPoseMarkers_); ROS_DEBUG ("Published ar_multi markers"); }
void ARSinglePublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg) { ARUint8 *dataPtr; ARMarkerInfo *marker_info; int marker_num; int i, k; /* 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); //flip image dataPtr = (ARUint8 *) capture_->imageData; // detect the markers in the video frame if (arDetectMarker (dataPtr, threshold_, &marker_info, &marker_num) < 0) { ROS_FATAL ("arDetectMarker failed"); ROS_BREAK (); // FIXME: I don't think this should be fatal... -Bill } // check for known patterns k = -1; for (i = 0; i < marker_num; i++) { if (marker_info[i].id == patt_id_) { ROS_DEBUG ("Found pattern: %d ", patt_id_); // make sure you have the best pattern (highest confidence factor) if (k == -1) k = i; else if (marker_info[k].cf < marker_info[i].cf) k = i; } } if (k != -1) { // **** get the transformation between the marker and the real camera double arQuat[4], arPos[3]; if (!useHistory_ || contF == 0) arGetTransMat (&marker_info[k], marker_center_, markerWidth_, marker_trans_); else arGetTransMatCont (&marker_info[k], marker_trans_, marker_center_, markerWidth_, marker_trans_); contF = 1; //arUtilMatInv (marker_trans_, cam_trans); arUtilMat2QuatPos (marker_trans_, arQuat, arPos); // **** convert to ROS frame double quat[4], pos[3]; pos[0] = arPos[0] * AR_TO_ROS; pos[1] = arPos[1] * AR_TO_ROS; pos[2] = arPos[2] * AR_TO_ROS; quat[0] = -arQuat[0]; quat[1] = -arQuat[1]; quat[2] = -arQuat[2]; quat[3] = arQuat[3]; ROS_DEBUG (" QUAT: Pos x: %3.5f y: %3.5f z: %3.5f", pos[0], pos[1], pos[2]); ROS_DEBUG (" Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1], quat[2], quat[3]); // **** publish the marker ar_pose_marker_.header.frame_id = image_msg->header.frame_id; ar_pose_marker_.header.stamp = image_msg->header.stamp; ar_pose_marker_.id = marker_info->id; ar_pose_marker_.pose.pose.position.x = pos[0]; ar_pose_marker_.pose.pose.position.y = pos[1]; ar_pose_marker_.pose.pose.position.z = pos[2]; ar_pose_marker_.pose.pose.orientation.x = quat[0]; ar_pose_marker_.pose.pose.orientation.y = quat[1]; ar_pose_marker_.pose.pose.orientation.z = quat[2]; ar_pose_marker_.pose.pose.orientation.w = quat[3]; ar_pose_marker_.confidence = marker_info->cf; arMarkerPub_.publish(ar_pose_marker_); ROS_DEBUG ("Published ar_single marker"); // **** publish transform between camera and marker btQuaternion rotation (quat[0], quat[1], quat[2], quat[3]); btVector3 origin(pos[0], pos[1], pos[2]); btTransform t(rotation, origin); if(publishTf_) { if(reverse_transform_) { tf::StampedTransform markerToCam (t.inverse(), image_msg->header.stamp, markerFrame_.c_str(), image_msg->header.frame_id); broadcaster_.sendTransform(markerToCam); } else { tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame_.c_str()); broadcaster_.sendTransform(camToMarker); } } // **** publish visual marker if(publishVisualMarkers_) { btVector3 markerOrigin(0, 0, 0.25 * markerWidth_ * AR_TO_ROS); btTransform m(btQuaternion::getIdentity(), markerOrigin); btTransform markerPose = t * m; // marker pose in the camera frame tf::poseTFToMsg(markerPose, rvizMarker_.pose); rvizMarker_.header.frame_id = image_msg->header.frame_id; rvizMarker_.header.stamp = image_msg->header.stamp; rvizMarker_.id = 1; rvizMarker_.scale.x = 1.0 * markerWidth_ * AR_TO_ROS; rvizMarker_.scale.y = 1.0 * markerWidth_ * AR_TO_ROS; rvizMarker_.scale.z = 0.5 * markerWidth_ * AR_TO_ROS; rvizMarker_.ns = "basic_shapes"; rvizMarker_.type = visualization_msgs::Marker::CUBE; rvizMarker_.action = visualization_msgs::Marker::ADD; rvizMarker_.color.r = 0.0f; rvizMarker_.color.g = 1.0f; rvizMarker_.color.b = 0.0f; rvizMarker_.color.a = 1.0; rvizMarker_.lifetime = ros::Duration(); rvizMarkerPub_.publish(rvizMarker_); ROS_DEBUG ("Published visual marker"); } } else { contF = 0; ROS_DEBUG ("Failed to locate marker"); } }