int main(int argc, char **argv)
{
ros::init(argc, argv, "object_recognition");
ros::NodeHandle nh;
ros::NodeHandle nh_param("~");
// Create a ROS subscriber for the object and world keypoints and descriptors
sub_keypoint_object = nh.subscribe ("/object_recognition/object/keypoints", 1, object_keypoint_cb);
sub_keypoint_world = nh.subscribe ("/object_recognition/world/keypoints", 1, world_keypoint_cb );
sub_descriptors_object_shot352 = nh.subscribe ("/object_recognition/object/descriptors/Shot352" , 1, object_descriptor_shot352_cb);
sub_descriptors_world_shot352 = nh.subscribe ("/object_recognition/world/descriptors/Shot352" , 1, world_descriptor_shot352_cb );
sub_descriptors_object_shot1344= nh.subscribe ("/object_recognition/object/descriptors/Shot1344", 1, object_descriptor_shot1344_cb);
sub_descriptors_world_shot1344 = nh.subscribe ("/object_recognition/world/descriptors/Shot1344" , 1, world_descriptor_shot1344_cb );
pub_object = nh.advertise<PointCloudROS> ("correspondences/object/clustered", 1);
pub_world = nh.advertise<PointCloudROS> ("correspondences/world/clustered", 1);
pub_object2 = nh.advertise<PointCloudROS> ("correspondences/object", 1);
pub_world2 = nh.advertise<PointCloudROS> ("correspondences/world", 1);
ros::spin();
return 0;
}
///\brief Opens joystick port, reads from port and publishes while node is active
int main(int argc, char **argv)
{
diagnostic_.add("Joystick Driver Status", this, &Joystick::diagnostics);
diagnostic_.setHardwareID("none");
// Parameters
ros::NodeHandle nh_param("~");
pub_ = nh_.advertise<fmMsgs::Joy>("joy", 1);
nh_param.param<std::string>("dev", joy_dev_, "/dev/input/js0");
nh_param.param<double>("deadzone", deadzone_, 0.05);
nh_param.param<double>("autorepeat_rate", autorepeat_rate_, 0);
nh_param.param<double>("coalesce_interval", coalesce_interval_, 0.001);
// Checks on parameters
if (autorepeat_rate_ > 1 / coalesce_interval_)
ROS_WARN("joy_node: autorepeat_rate (%f Hz) > 1/coalesce_interval (%f Hz) does not make sense. Timing behavior is not well defined.", autorepeat_rate_, 1/coalesce_interval_);
if (deadzone_ >= 1)
{
ROS_WARN("joy_node: deadzone greater than 1 was requested. The semantics of deadzone have changed. It is now related to the range [-1:1] instead of [-32767:32767]. For now I am dividing your deadzone by 32767, but this behavior is deprecated so you need to update your launch file.");
deadzone_ /= 32767;
}
if (deadzone_ > 0.9)
{
ROS_WARN("joy_node: deadzone (%f) greater than 0.9, setting it to 0.9", deadzone_);
deadzone_ = 0.9;
}
if (deadzone_ < 0)
{
ROS_WARN("joy_node: deadzone_ (%f) less than 0, setting to 0.", deadzone_);
deadzone_ = 0;
}
if (autorepeat_rate_ < 0)
{
ROS_WARN("joy_node: autorepeat_rate (%f) less than 0, setting to 0.", autorepeat_rate_);
autorepeat_rate_ = 0;
}
if (coalesce_interval_ < 0)
{
ROS_WARN("joy_node: coalesce_interval (%f) less than 0, setting to 0.", coalesce_interval_);
coalesce_interval_ = 0;
}
// Parameter conversions
double autorepeat_interval = 1 / autorepeat_rate_;
double scale = -1. / (1. - deadzone_) / 32767.;
double unscaled_deadzone = 32767. * deadzone_;
js_event event;
struct timeval tv;
fd_set set;
int joy_fd;
event_count_ = 0;
pub_count_ = 0;
lastDiagTime_ = ros::Time::now().toSec();
// Big while loop opens, publishes
while (nh_.ok())
{
open_ = false;
diagnostic_.force_update();
bool first_fault = true;
while (true)
{
ros::spinOnce();
if (!nh_.ok())
goto cleanup;
joy_fd = open(joy_dev_.c_str(), O_RDONLY);
if (joy_fd != -1)
{
// There seems to be a bug in the driver or something where the
// initial events that are to define the initial state of the
// joystick are not the values of the joystick when it was opened
// but rather the values of the joystick when it was last closed.
// Opening then closing and opening again is a hack to get more
// accurate initial state data.
close(joy_fd);
joy_fd = open(joy_dev_.c_str(), O_RDONLY);
}
if (joy_fd != -1)
break;
if (first_fault)
{
ROS_ERROR("Couldn't open joystick %s. Will retry every second.", joy_dev_.c_str());
first_fault = false;
}
sleep(1.0);
diagnostic_.update();
}
ROS_INFO("Opened joystick: %s. deadzone_: %f.", joy_dev_.c_str(), deadzone_);
open_ = true;
diagnostic_.force_update();
bool tv_set = false;
bool publication_pending = false;
tv.tv_sec = 1;
tv.tv_usec = 0;
fmMsgs::Joy joy_msg; // Here because we want to reset it on device close.
while (nh_.ok())
{
ros::spinOnce();
bool publish_now = false;
bool publish_soon = false;
FD_ZERO(&set);
FD_SET(joy_fd, &set);
//ROS_INFO("Select...");
int select_out = select(joy_fd+1, &set, NULL, NULL, &tv);
//ROS_INFO("Tick...");
if (select_out == -1)
{
tv.tv_sec = 0;
tv.tv_usec = 0;
//ROS_INFO("Select returned negative. %i", ros::isShuttingDown());
continue;
// break; // Joystick is probably closed. Not sure if this case is useful.
}
if (FD_ISSET(joy_fd, &set))
{
if (read(joy_fd, &event, sizeof(js_event)) == -1 && errno != EAGAIN)
break; // Joystick is probably closed. Definitely occurs.
//ROS_INFO("Read data...");
joy_msg.header.stamp = ros::Time().now();
event_count_++;
switch(event.type)
{
case JS_EVENT_BUTTON:
case JS_EVENT_BUTTON | JS_EVENT_INIT:
if(event.number >= joy_msg.buttons.size())
{
int old_size = joy_msg.buttons.size();
joy_msg.buttons.resize(event.number+1);
for(unsigned int i=old_size;i<joy_msg.buttons.size();i++)
joy_msg.buttons[i] = 0.0;
}
joy_msg.buttons[event.number] = (event.value ? 1 : 0);
// For initial events, wait a bit before sending to try to catch
// all the initial events.
if (!(event.type & JS_EVENT_INIT))
publish_now = true;
else
publish_soon = true;
break;
case JS_EVENT_AXIS:
case JS_EVENT_AXIS | JS_EVENT_INIT:
if(event.number >= joy_msg.axes.size())
{
int old_size = joy_msg.axes.size();
joy_msg.axes.resize(event.number+1);
for(unsigned int i=old_size;i<joy_msg.axes.size();i++)
joy_msg.axes[i] = 0.0;
}
if (!(event.type & JS_EVENT_INIT)) // Init event.value is wrong.
{
double val = event.value;
// Allows deadzone to be "smooth"
if (val > unscaled_deadzone)
val -= unscaled_deadzone;
else if (val < -unscaled_deadzone)
val += unscaled_deadzone;
else
val = 0;
joy_msg.axes[event.number] = val * scale;
}
// Will wait a bit before sending to try to combine events.
publish_soon = true;
break;
default:
ROS_WARN("joy_node: Unknown event type. Please file a ticket. time=%u, value=%d, type=%Xh, number=%d", event.time, event.value, event.type, event.number);
break;
}
}
else if (tv_set) // Assume that the timer has expired.
publish_now = true;
if (publish_now)
{
// Assume that all the JS_EVENT_INIT messages have arrived already.
// This should be the case as the kernel sends them along as soon as
// the device opens.
//ROS_INFO("Publish...");
pub_.publish(joy_msg);
publish_now = false;
tv_set = false;
publication_pending = false;
publish_soon = false;
pub_count_++;
}
// If an axis event occurred, start a timer to combine with other
// events.
if (!publication_pending && publish_soon)
{
tv.tv_sec = trunc(coalesce_interval_);
tv.tv_usec = (coalesce_interval_ - tv.tv_sec) * 1e6;
publication_pending = true;
tv_set = true;
//ROS_INFO("Pub pending...");
}
// If nothing is going on, start a timer to do autorepeat.
if (!tv_set && autorepeat_rate_ > 0)
{
tv.tv_sec = trunc(autorepeat_interval);
tv.tv_usec = (autorepeat_interval - tv.tv_sec) * 1e6;
tv_set = true;
//ROS_INFO("Autorepeat pending... %i %i", tv.tv_sec, tv.tv_usec);
}
if (!tv_set)
{
tv.tv_sec = 1;
tv.tv_usec = 0;
}
diagnostic_.update();
} // End of joystick open loop.
close(joy_fd);
ros::spinOnce();
if (nh_.ok())
ROS_ERROR("Connection to joystick device lost unexpectedly. Will reopen.");
}
cleanup:
ROS_INFO("joy_node shut down.");
return 0;
}
// callback function when the object descriptors are received
// this will also trigger the recognition if all the other keypoints and descriptors have been received
void object_descriptor_shot352_cb (const object_recognition::Shot352_bundle::Ptr input)
{
ros::NodeHandle nh_param("~");
nh_param.param<double>("maximum_descriptor_distance" , max_descr_dist_ , 0.25 );
// check if world was already processed
if (world_descriptors_shot352 == NULL)
{
ROS_WARN("Received object descriptors before having a world pointcloud to compare");
return;
}
// check if the stored world descriptors can be assinged to the stored keypoints
if ((int)world_keypoints->size() != (int)world_descriptors_shot352->size())
{
ROS_WARN("Received %i descriptors and %i keypoints for the world. Number must be equal", (int)world_descriptors_shot352->size(), (int)world_keypoints->size());
return;
}
// check if the received object descriptors can be assigned to the stored keypoints
if ((int)object_keypoints->size() != (int)input->descriptors.size())
{
ROS_WARN("Received %i descriptors and %i keypoints for the object. Number must be equal", (int)input->descriptors.size(), (int)object_keypoints->size());
return;
}
// Debug output
ROS_INFO("Received %i descriptors for the world and %i for the object", (int)world_descriptors_shot352->size(), (int)input->descriptors.size());
object_descriptors_shot352 = DescriptorCloudShot352::Ptr (new DescriptorCloudShot352 ());
fromROSMsg(*input, *object_descriptors_shot352);
//
// Find Object-World Correspondences with KdTree
//
cout << "... finding correspondences ..." << endl;
pcl::CorrespondencesPtr object_world_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<SHOT352> match_search;
match_search.setInputCloud (object_descriptors_shot352);
// For each world keypoint descriptor
// find nearest neighbor into the object keypoints descriptor cloud
// and add it to the correspondences vector
for (size_t i = 0; i < world_descriptors_shot352->size (); ++i)
{
std::vector<int> neigh_indices (1);
std::vector<float> neigh_sqr_dists (1);
if (!pcl_isfinite (world_descriptors_shot352->at (i).descriptor[0])) //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch (world_descriptors_shot352->at (i), 1, neigh_indices, neigh_sqr_dists);
// add match only if the squared descriptor distance is less than 0.25
// SHOT descriptor distances are between 0 and 1 by design
if(found_neighs == 1 && neigh_sqr_dists[0] < (float)max_descr_dist_)
{
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
object_world_corrs->push_back (corr);
}
}
std::cout << "Correspondences found: " << object_world_corrs->size () << std::endl;
//
// all keypoints and descriptors were found, no match the correspondences to the real object!
//
cluster(object_world_corrs);
}
void cluster(const pcl::CorrespondencesPtr &object_world_corrs)
{
ros::NodeHandle nh_param("~");
nh_param.param<double>("cg_size" , cg_size_ , 0.01 );
nh_param.param<double>("cg_thresh", cg_thresh_, 5.0);
//
// Debug output
//
PointCloud correspondence_object;
PointCloud correspondence_world;
for (int j = 0; j < object_world_corrs->size (); ++j)
{
PointType& object_point = object_keypoints->at(object_world_corrs->at(j).index_query);
PointType& world_point = world_keypoints->at(object_world_corrs->at(j).index_match);
correspondence_object.push_back(object_point);
correspondence_world.push_back(world_point);
// cout << object_point.x << " " << object_point.y << " " << object_point.z << endl;
// cout << world_point.x << " " << world_point.y << " " << world_point.z << endl;
}
PointCloudROS pub_me_object2;
PointCloudROS pub_me_world2;
toROSMsg(correspondence_object, pub_me_object2);
toROSMsg(correspondence_world, pub_me_world2);
pub_me_object2.header.frame_id = "/object";
pub_me_world2.header.frame_id = "/world";
pub_object2.publish(pub_me_object2);
pub_world2.publish(pub_me_world2);
//
// Actual Clustering
//
cout << "... clustering ..." << endl;
std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > rototranslations;
std::vector<pcl::Correspondences> clustered_corrs;
pcl::GeometricConsistencyGrouping<PointType, PointType> gc_clusterer;
gc_clusterer.setGCSize (cg_size_);
gc_clusterer.setGCThreshold (cg_thresh_);
gc_clusterer.setInputCloud (object_keypoints);
gc_clusterer.setSceneCloud (world_keypoints);
gc_clusterer.setModelSceneCorrespondences (object_world_corrs);
gc_clusterer.recognize (rototranslations, clustered_corrs);
//
// Output results
//
std::cout << "Object instances found: " << rototranslations.size () << std::endl;
int maximum = 0;
int best;
for (int i = 0; i < rototranslations.size (); ++i)
{
cout << "Instance "<< i << " has " << clustered_corrs[i].size () << " correspondences" << endl;
if (maximum < clustered_corrs[i].size ())
{
maximum = clustered_corrs[i].size ();
best = i;
}
}
if (rototranslations.size () > 0)
{
cout << "selecting instance " << best << " and calculating TF" << endl;
// Print the rotation matrix and translation vector
Eigen::Matrix3f rotation = rototranslations[best].block<3,3>(0, 0);
Eigen::Vector3f translation = rototranslations[best].block<3,1>(0, 3);
printf ("\n");
printf (" | %6.3f %6.3f %6.3f | \n", rotation (0,0), rotation (0,1), rotation (0,2));
printf (" R = | %6.3f %6.3f %6.3f | \n", rotation (1,0), rotation (1,1), rotation (1,2));
printf (" | %6.3f %6.3f %6.3f | \n", rotation (2,0), rotation (2,1), rotation (2,2));
printf ("\n");
printf (" t = < %0.3f, %0.3f, %0.3f >\n", translation (0), translation (1), translation (2));
// convert Eigen matricies into ROS TF message
tf::Vector3 object_offset;
tf::Quaternion object_rotation;
object_offset[0] = translation (0);
object_offset[1] = translation (1);
object_offset[2] = translation (2);
// convert rotation matrix to quaternion
Eigen::Quaternionf quaternion (rotation);
object_rotation[0] = quaternion.x();
object_rotation[1] = quaternion.y();
object_rotation[2] = quaternion.z();
object_rotation[3] = quaternion.w();
static tf::TransformBroadcaster br;
tf::Transform transform;
transform.setOrigin (object_offset);
transform.setRotation (object_rotation);
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "object"));
//
// Debug output
//
PointCloud correspondence_object_cluster;
PointCloud correspondence_world_cluster;
for (int j = 0; j < clustered_corrs[best].size (); ++j)
{
PointType& model_point = object_keypoints->at(clustered_corrs[best][j].index_query);
PointType& scene_point = world_keypoints->at(clustered_corrs[best][j].index_match);
correspondence_object_cluster.push_back(model_point);
correspondence_world_cluster.push_back(scene_point);
//cout << model_point.x << " " << model_point.y << " " << model_point.z << endl;
//cout << scene_point.x << " " << scene_point.y << " " << scene_point.z << endl;
}
PointCloudROS pub_me_object;
PointCloudROS pub_me_world;
toROSMsg(correspondence_object_cluster, pub_me_object);
toROSMsg(correspondence_world_cluster, pub_me_world);
pub_me_object.header.frame_id = "/object";
pub_me_world.header.frame_id = "/world";
pub_object.publish(pub_me_object);
pub_world.publish(pub_me_world);
}
}
// callback function when the object descriptors are received
// this will also trigger the recognition if all the other keypoints and descriptors have been received
void object_descriptor_shot1344_cb (const object_recognition::Shot1344_bundle::Ptr input)
{
ros::NodeHandle nh_param("~");
nh_param.param<double>("maximum_descriptor_distance" , max_descr_dist_ , 0.25 );
// check if world was already processed
if (world_descriptors_shot1344 == NULL)
{
ROS_WARN("Received object descriptors before having a world pointcloud to compare");
return;
}
// check if the stored world descriptors can be assinged to the stored keypoints
if ((int)world_keypoints->size() != (int)world_descriptors_shot1344->size())
{
ROS_WARN("Received %i descriptors and %i keypoints for the world. Number must be equal", (int)world_descriptors_shot1344->size(), (int)world_keypoints->size());
return;
}
// check if the received object descriptors can be assigned to the stored keypoints
if ((int)object_keypoints->size() != (int)input->descriptors.size())
{
ROS_WARN("Received %i descriptors and %i keypoints for the object. Number must be equal", (int)input->descriptors.size(), (int)object_keypoints->size());
return;
}
object_descriptors_shot1344 = DescriptorCloudShot1344::Ptr (new DescriptorCloudShot1344 ());
fromROSMsg(*input, *object_descriptors_shot1344);
// Debug output
ROS_INFO("Received %i descriptors for the world and %i for the object", (int)world_descriptors_shot1344->size(), (int)input->descriptors.size());
//
// Find Object-World Correspondences with KdTree
//
cout << "... finding correspondences ..." << endl;
pcl::CorrespondencesPtr object_world_corrs (new pcl::Correspondences ());
pcl::KdTreeFLANN<SHOT1344> match_search;
match_search.setInputCloud (object_descriptors_shot1344);
// For each world keypoint descriptor
// find nearest neighbor into the object keypoints descriptor cloud
// and add it to the correspondences vector
for (size_t i = 0; i < world_descriptors_shot1344->size (); ++i)
{
std::vector<int> neigh_indices (1);
std::vector<float> neigh_sqr_dists (1);
if (!pcl_isfinite (world_descriptors_shot1344->at (i).descriptor[0])) //skipping NaNs
{
continue;
}
int found_neighs = match_search.nearestKSearch (world_descriptors_shot1344->at (i), 1, neigh_indices, neigh_sqr_dists);
// add match only if the squared descriptor distance is less than max_descr_dist_
// SHOT descriptor distances are between 0 and 1 by design
if(found_neighs == 1 && neigh_sqr_dists[0] < (float)max_descr_dist_)
{
pcl::Correspondence corr (neigh_indices[0], static_cast<int> (i), neigh_sqr_dists[0]);
// check if new correspondence is better than any previous at this point
bool found_better_result = false;
for (int j = 0; j < object_world_corrs->size(); ++j)
{
// is the found neigbor the same one like in the correspondence j
if (object_world_corrs->at(j).index_query == neigh_indices[0])
{
// do not add a new correspondence later
found_better_result = true;
// is the new distance smaller? (that means better)
if (neigh_sqr_dists[0] < object_world_corrs->at(j).distance)
{
// replace correspondence with better one
object_world_corrs->at(j) = corr;
}
else
// break out of inner loop to save time and try next keypoint
break;
}
}
// if this is a new correspondence, add a new correspondence at the end
if (!found_better_result)
object_world_corrs->push_back (corr);
}
}
std::cout << "Correspondences found: " << object_world_corrs->size () << std::endl;
//
// all correspondences were found, now match the correspondences to the real object!
//
cluster(object_world_corrs);
}
