In my configuration, I use the executable IndividualMarkers (2D + 3D). I have noticed that when markers are near the 2D image borders, it will localize them very badly. A fast solution that I found was to modify IndividualMarkers.cpp. I'm not doing a pull request, since I have hardcoded the borders and the size og the kinect image, but I thought that it could possibly help someone wondering why the localization is not working. Here's the code modification for the point cloud callback :
void getPointCloudCallback (const sensor_msgs::PointCloud2ConstPtr &msg)
{
sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
//If we've already gotten the cam info, then go ahead
if(cam->getCamInfo_){
//Convert cloud to PCL
ARCloud cloud;
pcl::fromROSMsg(*msg, cloud);
//Get an OpenCV image from the cloud
pcl::toROSMsg (*msg, *image_msg);
image_msg->header.stamp = msg->header.stamp;
image_msg->header.frame_id = msg->header.frame_id;
//Convert the image
cv_ptr_ = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
//Get the estimated pose of the main markers by using all the markers in each bundle
// GetMultiMarkersPoses expects an IplImage*, but as of ros groovy, cv_bridge gives
// us a cv::Mat. I'm too lazy to change to cv::Mat throughout right now, so I
// do this conversion here -jbinney
IplImage ipl_image = cv_ptr_->image;
//Use the kinect to improve the pose
Pose ret_pose;
GetMarkerPoses(&ipl_image, cloud);
try{
tf::StampedTransform CamToOutput;
try{
tf_listener->waitForTransform(output_frame, image_msg->header.frame_id, image_msg->header.stamp, ros::Duration(1.0));
tf_listener->lookupTransform(output_frame, image_msg->header.frame_id, image_msg->header.stamp, CamToOutput);
}
catch (tf::TransformException ex){
ROS_ERROR("%s",ex.what());
}
arPoseMarkers_.markers.clear ();
for (size_t i=0; i<marker_detector.markers->size(); i++)
{
//Get the pose relative to the camera
int id = (*(marker_detector.markers))[i].GetId();
Pose p = (*(marker_detector.markers))[i].pose;
/// HERE IS THE ADDED CODE
/// YOU CAN NOTICE I REMOVE MARKERS WHICH CORNERS ARE 40PX FROM
/// KINECT IMAGE BORDERS
MarkerData tag_data = (*(marker_detector.markers))[i];
std::vector<PointDouble> corners_pts = tag_data.marker_corners_img;
bool point_is_near_border = false;
// Do not publish points near edges since it is a source of error
for(int p = 0; p < corners_pts.size(); p++){
PointDouble pt = corners_pts.at(p);
if(pt.x < 40 || pt.x > 600){
point_is_near_border = true;
break;
}
else if(pt.y < 40 || pt.y > 440){
point_is_near_border = true;
break;
}
}
if(!point_is_near_border){
double px = p.translation[0]/100.0;
double py = p.translation[1]/100.0;
double pz = p.translation[2]/100.0;
double qx = p.quaternion[1];
double qy = p.quaternion[2];
double qz = p.quaternion[3];
double qw = p.quaternion[0];
tf::Quaternion rotation (qx,qy,qz,qw);
tf::Vector3 origin (px,py,pz);
tf::Transform t (rotation, origin);
tf::Vector3 markerOrigin (0, 0, 0);
tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
tf::Transform markerPose = t * m; // marker pose in the camera frame
//Publish the transform from the camera to the marker
std::string markerFrame;
if(prefix == "") markerFrame = "ar3d_";
else markerFrame = prefix + "/" + markerFrame;
std::stringstream out;
out << id;
std::string id_string = out.str();
markerFrame += id_string;
tf::StampedTransform camToMarker (t, image_msg->header.stamp, image_msg->header.frame_id, markerFrame.c_str());
tf_broadcaster->sendTransform(camToMarker);
//Create the rviz visualization messages
tf::poseTFToMsg (markerPose, rvizMarker_.pose);
rvizMarker_.header.frame_id = image_msg->header.frame_id;
rvizMarker_.header.stamp = image_msg->header.stamp;
rvizMarker_.id = id;
rvizMarker_.scale.x = 1.0 * marker_size/100.0;
rvizMarker_.scale.y = 1.0 * marker_size/100.0;
rvizMarker_.scale.z = 0.2 * marker_size/100.0;
rvizMarker_.ns = "basic_shapes";
rvizMarker_.type = visualization_msgs::Marker::CUBE;
rvizMarker_.action = visualization_msgs::Marker::ADD;
switch (id)
{
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;
case 2:
rvizMarker_.color.r = 0.0f;
rvizMarker_.color.g = 1.0f;
rvizMarker_.color.b = 0.0f;
rvizMarker_.color.a = 1.0;
break;
case 3:
rvizMarker_.color.r = 0.0f;
rvizMarker_.color.g = 0.5f;
rvizMarker_.color.b = 0.5f;
rvizMarker_.color.a = 1.0;
break;
case 4:
rvizMarker_.color.r = 0.5f;
rvizMarker_.color.g = 0.5f;
rvizMarker_.color.b = 0.0;
rvizMarker_.color.a = 1.0;
break;
default:
rvizMarker_.color.r = 0.5f;
rvizMarker_.color.g = 0.0f;
rvizMarker_.color.b = 0.5f;
rvizMarker_.color.a = 1.0;
break;
}
rvizMarker_.lifetime = ros::Duration (1.0);
rvizMarkerPub_.publish (rvizMarker_);
//Get the pose of the tag in the camera frame, then the output frame (usually torso)
tf::Transform tagPoseOutput = CamToOutput * markerPose;
//Create the pose marker messages
ar_track_alvar_msgs::AlvarMarker ar_pose_marker;
tf::poseTFToMsg (tagPoseOutput, ar_pose_marker.pose.pose);
ar_pose_marker.header.frame_id = output_frame;
ar_pose_marker.header.stamp = image_msg->header.stamp;
ar_pose_marker.id = id;
arPoseMarkers_.markers.push_back (ar_pose_marker);
}
}
arPoseMarkers_.header.stamp = image_msg->header.stamp;
arMarkerPub_.publish (arPoseMarkers_);
}
catch (cv_bridge::Exception& e){
ROS_ERROR ("Could not convert from '%s' to 'rgb8'.", image_msg->encoding.c_str ());
}
}
}
_Copied from original issue: sniekum/ar_trackalvar#76
From @jpmerc on March 30, 2016 17:52
In my configuration, I use the executable IndividualMarkers (2D + 3D). I have noticed that when markers are near the 2D image borders, it will localize them very badly. A fast solution that I found was to modify IndividualMarkers.cpp. I'm not doing a pull request, since I have hardcoded the borders and the size og the kinect image, but I thought that it could possibly help someone wondering why the localization is not working. Here's the code modification for the point cloud callback :
_Copied from original issue: sniekum/ar_trackalvar#76