static void getResultQuat( ARMarkerInfo *marker_info ) { double target_trans[3][4]; double cam_trans[3][4]; double quat[4], pos[3]; char string1[256]; char string2[256]; if( arGetTransMat(marker_info, target_center, target_width, target_trans) < 0 ) return; if( arUtilMatInv(target_trans, cam_trans) < 0 ) return; if( arUtilMat2QuatPos(cam_trans, quat, pos) < 0 ) return; sprintf(string1," QUAT: Pos x: %3.1f y: %3.1f z: %3.1f\n", pos[0], pos[1], pos[2]); sprintf(string2, " Quat qx: %3.2f qy: %3.2f qz: %3.2f qw: %3.2f ", quat[0], quat[1], quat[2], quat[3]); strcat( string1, string2 ); if( disp_mode ) { draw( "target", target_trans, 0, 0 ); draw_exview( a, b, r, target_trans, 1, 1 ); } else { draw( "target", target_trans, 1, 1 ); draw_exview( a, b, r, target_trans, 0, 0 ); } print_string( string1 ); return; }
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"); }
int arFilterTransMat(ARFilterTransMatInfo *ftmi, ARdouble m[3][4], const int reset) { ARdouble q[4], p[3], alpha, oneminusalpha, omega, cosomega, sinomega, s0, s1; if (!ftmi) return (-1); if (arUtilMat2QuatPos(m, q, p) < 0) return (-2); arUtilQuatNorm(q); if (reset) { ftmi->q[0] = q[0]; ftmi->q[1] = q[1]; ftmi->q[2] = q[2]; ftmi->q[3] = q[3]; ftmi->p[0] = p[0]; ftmi->p[1] = p[1]; ftmi->p[2] = p[2]; } else { alpha = ftmi->alpha; #ifdef ARDOUBLE_IS_FLOAT oneminusalpha = 1.0f - alpha; #else oneminusalpha = 1.0 - alpha; #endif // SLERP for orientation. cosomega = q[0]*ftmi->q[0] + q[1]*ftmi->q[1] + q[2]*ftmi->q[2] + q[3]*ftmi->q[3]; // cos of angle between vectors. #ifdef ARDOUBLE_IS_FLOAT if (cosomega < 0.0f) { cosomega = -cosomega; q[0] = -q[0]; q[1] = -q[1]; q[2] = -q[2]; q[3] = -q[3]; } if (cosomega > 0.9995f) { s0 = oneminusalpha; s1 = alpha; } else { omega = acosf(cosomega); sinomega = sinf(omega); s0 = sinf(oneminusalpha * omega) / sinomega; s1 = sinf(alpha * omega) / sinomega; } #else if (cosomega < 0.0) { cosomega = -cosomega; q[0] = -q[0]; q[1] = -q[1]; q[2] = -q[2]; q[3] = -q[3]; } if (cosomega > 0.9995) { s0 = oneminusalpha; s1 = alpha; } else { omega = acos(cosomega); sinomega = sin(omega); s0 = sin(oneminusalpha * omega) / sinomega; s1 = sin(alpha * omega) / sinomega; } #endif ftmi->q[0] = q[0]*s1 + ftmi->q[0]*s0; ftmi->q[1] = q[1]*s1 + ftmi->q[1]*s0; ftmi->q[2] = q[2]*s1 + ftmi->q[2]*s0; ftmi->q[3] = q[3]*s1 + ftmi->q[3]*s0; arUtilQuatNorm(ftmi->q); // Linear interpolation for position. ftmi->p[0] = p[0]*alpha + ftmi->p[0]*oneminusalpha; ftmi->p[1] = p[1]*alpha + ftmi->p[1]*oneminusalpha; ftmi->p[2] = p[2]*alpha + ftmi->p[2]*oneminusalpha; } if (arUtilQuatPos2Mat(ftmi->q, ftmi->p, m) < 0) return (-2); return (0); }
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"); } }
void ARSinglePublisher::getTransformationCallback(const sensor_msgs::ImageConstPtr& image_msg) { // ROS_INFO("======================================================"); // ROS_INFO("Callback..."); ARMarkerInfo *marker_info; int marker_num; int i, k; // Get the image from ROSTOPIC // NOTE: the data_ptr format is BGR because the ARToolKit library was // build with V4L, data_ptr 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 ()); return; } // const int WIDTH = 640; // const int HEIGHT = 480; // // declare a destination IplImage object with correct size, depth and channels // IplImage *destination = cvCreateImage(cvSize(WIDTH, HEIGHT), capture_->depth, capture_->nChannels); // // use cvResize to resize source to a destination image // cvResize(capture_, destination); // // save image with a name supplied with a second argument // std::string filename = "/tmp/" + marker_frame_ + ".jpg"; // cvSaveImage(filename.c_str(), destination); // ROS_INFO("BEFORE: Depth = >%i<.", capture_->depth); // ROS_INFO("BEFORE: nChannels = >%i<.", capture_->nChannels); // ROS_INFO("BEFORE: Width = >%i<.", capture_->width); // ROS_INFO("BEFORE: WidthStep = >%i<.", capture_->widthStep); // ROS_INFO("BEFORE: Height = >%i<.", capture_->height); // ROS_INFO("BEFORE: ImageSize = >%i<.", capture_->imageSize); // ROS_INFO("BEFORE: nSize = >%i<.", capture_->nSize); // ROS_INFO("BEFORE: dataOrder = >%i<.", capture_->dataOrder); // ROS_INFO("BEFORE: origin = >%i<.", capture_->origin); // capture_ = destination; // // memcpy(capture_->imageData, destination->imageData, destination->imageSize); // ROS_INFO("AFTER: Depth = >%i<.", capture_->depth); // ROS_INFO("AFTER: nChannels = >%i<.", capture_->nChannels); // ROS_INFO("AFTER: Width = >%i<.", capture_->width); // ROS_INFO("AFTER: WidthStep = >%i<.", capture_->widthStep); // ROS_INFO("AFTER: Height = >%i<.", capture_->height); // ROS_INFO("AFTER: ImageSize = >%i<.", capture_->imageSize); // ROS_INFO("AFTER: nSize = >%i<.", capture_->nSize); // ROS_INFO("AFTER: dataOrder = >%i<.", capture_->dataOrder); // ROS_INFO("AFTER: origin = >%i<.", capture_->origin); // cvConvertImage(capture_, capture_, CV_CVTIMG_FLIP); //flip image ARUint8 *data_ptr = (ARUint8 *)capture_->imageData; // detect the markers in the video frame if (arDetectMarker(data_ptr, 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) { if (!use_history_ || cont_f_ == 0) { arGetTransMat(&marker_info[k], marker_center_, marker_width_, marker_trans_); } else { arGetTransMatCont(&marker_info[k], marker_trans_, marker_center_, marker_width_, marker_trans_); } cont_f_ = 1; // get the transformation between the marker and the real camera double arQuat[4], arPos[3]; // arUtilMatInv (marker_trans_, cam_trans); arUtilMat2QuatPos(marker_trans_, arQuat, arPos); // error checking if(fabs(sqrt(arQuat[0]*arQuat[0] + arQuat[1]*arQuat[1] + arQuat[2]*arQuat[2] + arQuat[3]*arQuat[3]) - 1.0) > 0.0001) { ROS_WARN("Skipping invalid frame. Computed quaternion is invalid."); } if(std::isnan(arQuat[0]) || std::isnan(arQuat[1]) || std::isnan(arQuat[2]) || std::isnan(arQuat[3])) { ROS_WARN("Skipping invalid frame because computed orientation is not a number."); return; } if(std::isinf(arQuat[0]) || std::isinf(arQuat[1]) || std::isinf(arQuat[2]) || std::isinf(arQuat[3])) { ROS_WARN("Skipping invalid frame because computed orientation is infinite."); return; } // 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(" 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_target_marker_.confidence = marker_info->cf; ar_marker_publisher_.publish(ar_target_marker_); ROS_DEBUG ("Published ar_single marker"); // publish transform between camera and marker tf::Quaternion rotation(quat[0], quat[1], quat[2], quat[3]); tf::Vector3 origin(pos[0], pos[1], pos[2]); tf::Transform transform(rotation, origin); // TODO: figure out why this happens once in a while... if(transform.getOrigin().getZ() < 0.0) { transform.setOrigin(-transform.getOrigin()); } if (publish_tf_) { if (reverse_transform_) { ROS_ASSERT_MSG(false, "Reverse transform is not debugged yet."); // tf::StampedTransform marker_to_cam(t.inverse(), image_msg->header.stamp, marker_frame_.c_str(), image_msg->header.frame_id); tf::StampedTransform marker_to_cam(transform.inverse(), image_msg->header.stamp, marker_frame_, camera_frame_); tf_broadcaster_.sendTransform(marker_to_cam); } else { tf::Transform offseted_transform = transform * marker_offset_; // tf::StampedTransform cam_to_marker(t, image_msg->header.stamp, image_msg->header.frame_id, marker_frame_.c_str()); tf::StampedTransform cam_to_marker(offseted_transform, image_msg->header.stamp, camera_frame_, marker_frame_); // tf::StampedTransform cam_to_marker(transform, image_msg->header.stamp, camera_frame_, marker_frame_); tf_broadcaster_.sendTransform(cam_to_marker); } } // publish visual marker publishMarker(transform, image_msg->header.stamp); } else { cont_f_ = 0; ROS_WARN("Failed to locate marker."); } }
void ARBundlePublisher::getTransformationCallback (const sensor_msgs::ImageConstPtr & image_msg) { ARUint8 *dataPtr; ARMarkerInfo *marker_info; int marker_num; int knownPatternCount, 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 (knownPatternCount = 0; knownPatternCount < objectnum; knownPatternCount++) { k = -1; //haven't yet seen my pattern yet. //marker_num is how many markers were actually found for (j = 0; j < marker_num; j++) { if (object[knownPatternCount].id == marker_info[j].id) { if (k == -1) //if this is the first wild sighting k = j; //which marker matches my pattern? else // make sure you have the best pattern (highest confidence factor) if (marker_info[k].cf < marker_info[j].cf) k = j; } }//end for (j) if (k == -1) //didn't find my pattern :( { object[knownPatternCount].visible = 0; continue; //ok. so this just skips all the way to the next knownPatternCount } object[knownPatternCount].visible = 1; //woohoo mark as found getMarkerTransform(knownPatternCount, marker_info, k); //transform is stored in object[knownPatternCount].trans double arQuat[4], arPos[3]; //for the marker double masterARQuat[4], masterARPos[3]; //for the master/center of the box //find the transform for the pattern to the center of the box //updates master_trans_ findTransformToCenter(object[knownPatternCount].trans, knownPatternCount); //arUtilMatInv (object[i].trans, cam_trans); arUtilMat2QuatPos (object[knownPatternCount].trans, arQuat, arPos); arUtilMat2QuatPos (master_trans_, masterARQuat, masterARPos); // **** convert to ROS frame double quat[4], pos[3]; double masterQuat[4], masterPos[3]; convertToRosFrame(arQuat, arPos, quat, pos); convertToRosFrame(masterARQuat, masterARPos, masterQuat, masterPos); //write pos and quat out to text file if (outputToFile) { //need to output video tick //i know this is incredibly incredibly not efficient :( //TODO: output tick as well, from the camera_raw msg //convert nanoseconds -> seconds snprintf(buffer, BUFFER_SIZE, "%10.10f", image_msg->header.stamp.toNSec()*1e-9); output << buffer << ","; output << masterPos[0] << ","; output << masterPos[1] << ","; output << masterPos[2] << ","; output << masterQuat[0] << ","; output << masterQuat[1] << ","; output << masterQuat[2] << ","; output << masterQuat[3] << "\n"; } ROS_DEBUG (" Object num %i------------------------------------------------", knownPatternCount); 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]); // **** prepare to publish the marker stuffARMarkerMsg(knownPatternCount, pos, quat,image_msg->header, marker_info); // **** 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 tf::Quaternion masterRotation (masterQuat[0], masterQuat[1], masterQuat[2], masterQuat[3]); tf::Vector3 masterOrigin (masterPos[0], masterPos[1], masterPos[2]); tf::Transform masterTransform (masterRotation, masterOrigin); if (publishTf_) { //no need to publish tfs of markers // tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, object[knownPatternCount].name); // broadcaster_.sendTransform(camToMarker); tf::StampedTransform camToMaster (masterTransform, image_msg->header.stamp, image_msg->header.frame_id, "master"); broadcaster_.sendTransform(camToMaster); } // **** publish visual marker if (publishVisualMarkers_) { #if ROS_VERSION_MINIMUM(1, 9, 0) tf::Vector3 markerOrigin (0, 0, 0.25 * object[knownPatternCount].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 publishVisualMarker(knownPatternCount, markerPose, image_msg->header); } } //end outer loop of for arMarkerPub_.publish(arPoseMarkers_); }
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); // } } } }