void stereoSelectorCallback(const sensor_msgs::Image::ConstPtr& image_ptr)
{
if(!capture_image)
return;
cv_bridge::CvImagePtr cv_ptr;
try
{
cv_ptr = cv_bridge::toCvCopy(image_ptr, sensor_msgs::image_encodings::RGB8);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
cv::cvtColor(cv_ptr->image, input_image, CV_BGR2RGB);
string object_name;
//input_image = crop_hand(input_image);
float certainty = orbit->recognizeObject(input_image, object_name, Orbit::BAG_OF_WORDS_SVM);
/*
* Clean stabilizer if gesture has not been seen in a while
*/
ros::Time now = ros::Time::now();
ros::Duration diff_last_hand_received = now - last_hand_received;
last_hand_received = now;
if(diff_last_hand_received.toSec()>1)
{
for(unsigned int i = 0; i<stabilizer.size(); i++)
{
stabilizer[i] = 0;
}
}
/*
* Update stabilizer when the gesture is not recognized
*/
if(certainty<(float)certainty_threshold)
{
for(unsigned int i = 0; i<stabilizer.size()-1; i++)
{
if(stabilizer[i]>0)
stabilizer[i]--;
}
if(stabilizer[stabilizer.size()-1] < max_stabilizer)
stabilizer[stabilizer.size()-1]++;
return;
}
else
{
if(stabilizer[stabilizer.size()-1] >= 2)
stabilizer[stabilizer.size()-1]-=2;
else if(stabilizer[stabilizer.size()-1] == 1)
stabilizer[stabilizer.size()-1]--;
}
/*
* Update stabilizer when gesture is known
*/
for(unsigned int i = 0; i<stabilizer.size()-1; i++)
{
if(object_name == hands[i])
{
if(stabilizer[i] < max_stabilizer)
stabilizer[i]++;
}
else
{ if(stabilizer[i]>0)
stabilizer[i]--;
}
}
/*
* Print Stabilizer values
*/
for(unsigned int i = 0; i<stabilizer.size(); i++)
{
if(i<stabilizer.size()-1)
printf("%s: %d, ",hands[i].c_str(), stabilizer[i]);
else
printf("Not known: %d\n", stabilizer[i]);
}
/*
* Find maximum element of the stabilizer
*/
int max_stab = stabilizer[0];
unsigned int max_index = 0;
for(unsigned int i = 0; i<stabilizer.size(); i++)
{
if(stabilizer[i]>max_stab)
{
max_stab = stabilizer[i];
max_index = i;
}
}
ros::Duration dur = now-last_pub;
if(max_stab >= stabilizer_threshold && dur.toSec()>2 && max_index < stabilizer.size()-1)
{
std_msgs::String msg_to_sent;
msg_to_sent.data = hands[max_index];
hand_pub.publish(msg_to_sent);
last_pub = now;
for(unsigned int i = 0; i<stabilizer.size(); i++)
{
stabilizer[i] = 0;
}
}
}
