/*
* One and only one callback, now takes cloud, does everything else needed.
*/
void ARPublisher::getTransformationCallback (const sensor_msgs::PointCloud2ConstPtr & msg)
{
sensor_msgs::ImagePtr image_msg(new sensor_msgs::Image);
ARUint8 *dataPtr;
ARMarkerInfo *marker_info;
int marker_num;
int i, k, j;
/* do we need to initialize? */
if(!configured_)
{
if(msg->width == 0 || msg->height == 0)
{
ROS_ERROR ("Deformed cloud! Size = %d, %d.", msg->width, msg->height);
return;
}
cam_param_.xsize = msg->width;
cam_param_.ysize = msg->height;
cam_param_.dist_factor[0] = msg->width/2; // x0 = cX from openCV calibration
cam_param_.dist_factor[1] = msg->height/2; // y0 = cY from openCV calibration
cam_param_.dist_factor[2] = 0; // f = -100*k1 from CV. Note, we had to do mm^2 to m^2, hence 10^8->10^2
cam_param_.dist_factor[3] = 1.0; // scale factor, should probably be >1, but who cares...
arInit ();
}
/* convert cloud to PCL */
PointCloud cloud;
pcl::fromROSMsg(*msg, cloud);
/* get an OpenCV image from the cloud */
pcl::toROSMsg (cloud, *image_msg);
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR ("Could not convert from '%s' to 'bgr8'.", image_msg->encoding.c_str ());
}
dataPtr = (ARUint8 *) cv_ptr->image.ptr();
/* detect the markers in the video frame */
if (arDetectMarkerLite (dataPtr, threshold_, &marker_info, &marker_num) < 0)
{
argCleanup ();
return;
}
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;
}
/* create a cloud for marker corners */
int d = marker_info[k].dir;
PointCloud marker;
marker.push_back( cloud.at( (int)marker_info[k].vertex[(4-d)%4][0], (int)marker_info[k].vertex[(4-d)%4][1] ) ); // upper left
marker.push_back( cloud.at( (int)marker_info[k].vertex[(5-d)%4][0], (int)marker_info[k].vertex[(5-d)%4][1] ) ); // upper right
marker.push_back( cloud.at( (int)marker_info[k].vertex[(6-d)%4][0], (int)marker_info[k].vertex[(6-d)%4][1] ) ); // lower right
marker.push_back( cloud.at( (int)marker_info[k].vertex[(7-d)%4][0], (int)marker_info[k].vertex[(7-d)%4][1] ) );
/* create an ideal cloud */
double w = object[i].marker_width;
PointCloud ideal;
ideal.push_back( makeRGBPoint(-w/2,w/2,0) );
ideal.push_back( makeRGBPoint(w/2,w/2,0) );
ideal.push_back( makeRGBPoint(w/2,-w/2,0) );
ideal.push_back( makeRGBPoint(-w/2,-w/2,0) );
/* get transformation */
Eigen::Matrix4f t;
TransformationEstimationSVD obj;
obj.estimateRigidTransformation( marker, ideal, t );
/* get final transformation */
tf::Transform transform = tfFromEigen(t.inverse());
// any(transform == nan)
tf::Matrix3x3 m = transform.getBasis();
tf::Vector3 p = transform.getOrigin();
bool invalid = false;
for(int i=0; i < 3; i++)
for(int j=0; j < 3; j++)
invalid = (invalid || isnan(m[i][j]) || fabs(m[i][j]) > 1.0);
for(int i=0; i < 3; i++)
invalid = (invalid || isnan(p[i]));
if(invalid)
continue;
/* publish the marker */
ar_pose::ARMarker ar_pose_marker;
ar_pose_marker.header.frame_id = msg->header.frame_id;
ar_pose_marker.header.stamp = msg->header.stamp;
ar_pose_marker.id = object[i].id;
ar_pose_marker.pose.pose.position.x = transform.getOrigin().getX();
ar_pose_marker.pose.pose.position.y = transform.getOrigin().getY();
ar_pose_marker.pose.pose.position.z = transform.getOrigin().getZ();
ar_pose_marker.pose.pose.orientation.x = transform.getRotation().getAxis().getX();
ar_pose_marker.pose.pose.orientation.y = transform.getRotation().getAxis().getY();
ar_pose_marker.pose.pose.orientation.z = transform.getRotation().getAxis().getZ();
ar_pose_marker.pose.pose.orientation.w = transform.getRotation().getW();
ar_pose_marker.confidence = marker_info->cf;
arPoseMarkers_.markers.push_back (ar_pose_marker);
/* publish transform */
if (publishTf_)
{
broadcaster_.sendTransform(tf::StampedTransform(transform, msg->header.stamp, msg->header.frame_id, object[i].name));
}
/* publish visual marker */
if (publishVisualMarkers_)
{
tf::Vector3 markerOrigin (0, 0, 0.25 * object[i].marker_width * AR_TO_ROS);
tf::Transform m (tf::Quaternion::getIdentity (), markerOrigin);
tf::Transform markerPose = transform * m; // marker pose in the camera frame
tf::poseTFToMsg (markerPose, rvizMarker_.pose);
rvizMarker_.header.frame_id = msg->header.frame_id;
rvizMarker_.header.stamp = 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 ();
rvizMarkerPub_.publish (rvizMarker_);
ROS_DEBUG ("Published visual marker");
}
}
arMarkerPub_.publish (arPoseMarkers_);
ROS_DEBUG ("Published ar_multi markers");
}
void pairwiseMatchingRANSAC(
const RGBDFrame& frame_a, const RGBDFrame& frame_b,
double max_eucl_dist_sq,
double max_desc_dist,
double sufficient_inlier_ratio,
int max_ransac_iterations,
std::vector<cv::DMatch>& all_matches,
std::vector<cv::DMatch>& best_inlier_matches,
Eigen::Matrix4f& best_transformation)
{
// params
bool use_ratio_test = true;
float max_desc_ratio = 0.75;
// constants
int min_sample_size = 3;
cv::FlannBasedMatcher matcher; // for SURF
TransformationEstimationSVD svd;
std::vector<cv::DMatch> candidate_matches;
// **** build candidate matches ***********************************
// assumes detectors and distributions are computed
// establish all matches from b to a
if (use_ratio_test)
{
std::vector<std::vector<cv::DMatch> > all_matches2;
matcher.knnMatch(
frame_b.descriptors, frame_a.descriptors, all_matches2, 2);
for (unsigned int m_idx = 0; m_idx < all_matches2.size(); ++m_idx)
{
const cv::DMatch& match1 = all_matches2[m_idx][0];
const cv::DMatch& match2 = all_matches2[m_idx][1];
double ratio = match1.distance / match2.distance;
// remove bad matches - ratio test, valid keypoints
if (ratio < max_desc_ratio)
{
int idx_b = match1.queryIdx;
int idx_a = match1.trainIdx;
if (frame_a.kp_valid[idx_a] && frame_b.kp_valid[idx_b])
candidate_matches.push_back(match1);
}
}
}
else
{
matcher.match(
frame_b.descriptors, frame_a.descriptors, all_matches);
for (unsigned int m_idx = 0; m_idx < all_matches.size(); ++m_idx)
{
const cv::DMatch& match = all_matches[m_idx];
// remove bad matches - descriptor distance, valid keypoints
if (match.distance < max_desc_dist)
{
int idx_b = match.queryIdx;
int idx_a = match.trainIdx;
if (frame_a.kp_valid[idx_a] && frame_b.kp_valid[idx_b])
candidate_matches.push_back(match);
}
}
}
int size = candidate_matches.size();
//printf("size: %d\n", size);
if (size < min_sample_size) return;
// **** build 3D features for SVD ********************************
PointCloudFeature features_a, features_b;
features_a.resize(size);
features_b.resize(size);
for (int m_idx = 0; m_idx < size; ++m_idx)
{
const cv::DMatch& match = candidate_matches[m_idx];
int idx_b = match.queryIdx;
int idx_a = match.trainIdx;
PointFeature& p_a = features_a[m_idx];
p_a.x = frame_a.kp_means[idx_a](0,0);
p_a.y = frame_a.kp_means[idx_a](1,0);
p_a.z = frame_a.kp_means[idx_a](2,0);
PointFeature& p_b = features_b[m_idx];
p_b.x = frame_b.kp_means[idx_b](0,0);
p_b.y = frame_b.kp_means[idx_b](1,0);
p_b.z = frame_b.kp_means[idx_b](2,0);
}
// **** main RANSAC loop ****************************************
int best_n_inliers = 0;
Eigen::Matrix4f transformation; // transformation used inside loop
for (int iteration = 0; iteration < max_ransac_iterations; ++iteration)
{
// generate random indices
IntVector sample_idx;
getRandomIndices(min_sample_size, size, sample_idx);
// build initial inliers from random indices
IntVector inlier_idx;
std::vector<cv::DMatch> inlier_matches;
for (unsigned int s_idx = 0; s_idx < sample_idx.size(); ++s_idx)
{
int m_idx = sample_idx[s_idx];
inlier_idx.push_back(m_idx);
inlier_matches.push_back(candidate_matches[m_idx]);
}
// estimate transformation from minimum set of random samples
svd.estimateRigidTransformation(
features_b, inlier_idx,
features_a, inlier_idx,
transformation);
// evaluate transformation fitness by checking distance to all points
PointCloudFeature features_b_tf;
pcl::transformPointCloud(features_b, features_b_tf, transformation);
for (int m_idx = 0; m_idx < size; ++m_idx)
{
const PointFeature& p_a = features_a[m_idx];
const PointFeature& p_b = features_b_tf[m_idx];
float dist_sq = distEuclideanSq(p_a, p_b);
if (dist_sq < max_eucl_dist_sq)
{
inlier_idx.push_back(m_idx);
inlier_matches.push_back(candidate_matches[m_idx]);
// reestimate transformation from all inliers
svd.estimateRigidTransformation(
features_b, inlier_idx,
features_a, inlier_idx,
transformation);
pcl::transformPointCloud(features_b, features_b_tf, transformation);
}
}
// check if inliers are better than the best model so far
int n_inliers = inlier_idx.size();
if (n_inliers > best_n_inliers)
{
svd.estimateRigidTransformation(
features_b, inlier_idx,
features_a, inlier_idx,
transformation);
best_n_inliers = n_inliers;
best_transformation = transformation;
best_inlier_matches = inlier_matches;
}
// check if we reached ratio termination criteria
double inlier_ratio = (double) n_inliers / (double) size;
if (inlier_ratio > sufficient_inlier_ratio)
break;
}
}
