LineMOD_Detector(): it(nh)
{
color_sub = it.subscribe("color", 1, &LineMOD_Detector::colorCallback, this);
depth_sub = nh.subscribe("depth", 1, &LineMOD_Detector::depthCallback, this);
disp_sub = nh.subscribe("disparity", 1, &LineMOD_Detector::disparityCallback, this);
cam_info_sub = nh.subscribe("cam_info", 1, &LineMOD_Detector::cameraInfoCallback, this);
img_point_pub = nh.advertise<samplereturn_msgs::NamedPoint>("img_point", 1);
point_pub = nh.advertise<samplereturn_msgs::NamedPoint>("point", 1);
matching_threshold = 80;
got_color = false;
K = cv::Mat(3,3,CV_64FC1);
inv_K = cv::Mat(3,3,CV_64FC1);
std::string filename;
ros::param::get("~template_file", filename);
ros::param::get("~pub_threshold", LineMOD_Detector::pub_threshold);
ros::param::get("~min_depth", LineMOD_Detector::min_depth);
ros::param::get("~max_depth", LineMOD_Detector::max_depth);
ros::param::get("~min_count", LineMOD_Detector::min_count);
ROS_DEBUG("Pub Threshold:%f ", LineMOD_Detector::pub_threshold);
// Initialize LINEMOD data structures
detector = readLinemod(filename);
num_modalities = (int)detector->getModalities().size();
std::cout << num_modalities << std::endl;
}
int main(int argc, char * argv[])
{
// Various settings and flags
bool show_match_result = true;
bool show_timings = false;
bool learn_online = false;
int num_classes = 0;
int matching_threshold = 80;
/// @todo Keys for changing these?
cv::Size roi_size(200, 200);
int learning_lower_bound = 90;
int learning_upper_bound = 95;
// Timers
Timer extract_timer;
Timer match_timer;
// Initialize HighGUI
help();
cv::namedWindow("color");
cv::namedWindow("normals");
Mouse::start("color");
// Initialize LINEMOD data structures
cv::Ptr<cv::linemod::Detector> detector;
std::string filename;
if (argc == 1)
{
filename = "linemod_templates.yml";
detector = cv::linemod::getDefaultLINEMOD();
}
else
{
detector = readLinemod(argv[1]);
std::vector<cv::String> ids = detector->classIds();
num_classes = detector->numClasses();
printf("Loaded %s with %d classes and %d templates\n",
argv[1], num_classes, detector->numTemplates());
if (!ids.empty())
{
printf("Class ids:\n");
std::copy(ids.begin(), ids.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
}
}
int num_modalities = (int)detector->getModalities().size();
// Open Kinect sensor
cv::VideoCapture capture(cv::CAP_OPENNI2);
if (!capture.isOpened())
{
printf("Could not open OpenNI-capable sensor\n");
return -1;
}
capture.set(cv::CAP_PROP_OPENNI_REGISTRATION, 1);
double focal_length = capture.get(cv::CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH);
//printf("Focal length = %f\n", focal_length);
// Main loop
cv::Mat color, depth;
for (;;)
{
// Capture next color/depth pair
capture.grab();
capture.retrieve(depth, cv::CAP_OPENNI_DEPTH_MAP);
capture.retrieve(color, cv::CAP_OPENNI_BGR_IMAGE);
std::vector<cv::Mat> sources;
sources.push_back(color);
sources.push_back(depth);
cv::Mat display = color.clone();
if (!learn_online)
{
cv::Point mouse(Mouse::x(), Mouse::y());
int event = Mouse::event();
// Compute ROI centered on current mouse location
cv::Point roi_offset(roi_size.width / 2, roi_size.height / 2);
cv::Point pt1 = mouse - roi_offset; // top left
cv::Point pt2 = mouse + roi_offset; // bottom right
if (event == cv::EVENT_RBUTTONDOWN)
{
// Compute object mask by subtracting the plane within the ROI
std::vector<CvPoint> chain(4);
chain[0] = pt1;
chain[1] = cv::Point(pt2.x, pt1.y);
chain[2] = pt2;
chain[3] = cv::Point(pt1.x, pt2.y);
cv::Mat mask;
subtractPlane(depth, mask, chain, focal_length);
cv::imshow("mask", mask);
// Extract template
std::string class_id = cv::format("class%d", num_classes);
cv::Rect bb;
extract_timer.start();
int template_id = detector->addTemplate(sources, class_id, mask, &bb);
extract_timer.stop();
if (template_id != -1)
{
printf("*** Added template (id %d) for new object class %d***\n",
template_id, num_classes);
//printf("Extracted at (%d, %d) size %dx%d\n", bb.x, bb.y, bb.width, bb.height);
}
++num_classes;
}
// Draw ROI for display
cv::rectangle(display, pt1, pt2, CV_RGB(0, 0, 0), 3);
cv::rectangle(display, pt1, pt2, CV_RGB(255, 255, 0), 1);
}
// Perform matching
std::vector<cv::linemod::Match> matches;
std::vector<cv::String> class_ids;
std::vector<cv::Mat> quantized_images;
match_timer.start();
detector->match(sources, (float)matching_threshold, matches, class_ids, quantized_images);
match_timer.stop();
int classes_visited = 0;
std::set<std::string> visited;
for (int i = 0; (i < (int)matches.size()) && (classes_visited < num_classes); ++i)
{
cv::linemod::Match m = matches[i];
if (visited.insert(m.class_id).second)
{
++classes_visited;
if (show_match_result)
{
printf("Similarity: %5.1f%%; x: %3d; y: %3d; class: %s; template: %3d\n",
m.similarity, m.x, m.y, m.class_id.c_str(), m.template_id);
}
// Draw matching template
const std::vector<cv::linemod::Template>& templates = detector->getTemplates(m.class_id, m.template_id);
drawResponse(templates, num_modalities, display, cv::Point(m.x, m.y), detector->getT(0));
if (learn_online == true)
{
/// @todo Online learning possibly broken by new gradient feature extraction,
/// which assumes an accurate object outline.
// Compute masks based on convex hull of matched template
cv::Mat color_mask, depth_mask;
std::vector<CvPoint> chain = maskfusionromTemplate(templates, num_modalities,
cv::Point(m.x, m.y), color.size(),
color_mask, display);
subtractPlane(depth, depth_mask, chain, focal_length);
cv::imshow("mask", depth_mask);
// If pretty sure (but not TOO sure), add new template
if (learning_lower_bound < m.similarity && m.similarity < learning_upper_bound)
{
extract_timer.start();
int template_id = detector->addTemplate(sources, m.class_id, depth_mask);
extract_timer.stop();
if (template_id != -1)
{
printf("*** Added template (id %d) for existing object class %s***\n",
template_id, m.class_id.c_str());
}
}
}
}
}
if (show_match_result && matches.empty())
printf("No matches found...\n");
if (show_timings)
{
printf("Training: %.2fs\n", extract_timer.time());
printf("Matching: %.2fs\n", match_timer.time());
}
if (show_match_result || show_timings)
printf("------------------------------------------------------------\n");
cv::imshow("color", display);
cv::imshow("normals", quantized_images[1]);
cv::FileStorage fs;
char key = (char)cv::waitKey(10);
if (key == 'q')
break;
switch (key)
{
case 'h':
help();
break;
case 'm':
// toggle printing match result
show_match_result = !show_match_result;
printf("Show match result %s\n", show_match_result ? "ON" : "OFF");
break;
case 't':
// toggle printing timings
show_timings = !show_timings;
printf("Show timings %s\n", show_timings ? "ON" : "OFF");
break;
case 'l':
// toggle online learning
learn_online = !learn_online;
printf("Online learning %s\n", learn_online ? "ON" : "OFF");
break;
case '[':
// decrement threshold
matching_threshold = std::max(matching_threshold - 1, -100);
printf("New threshold: %d\n", matching_threshold);
break;
case ']':
// increment threshold
matching_threshold = std::min(matching_threshold + 1, +100);
printf("New threshold: %d\n", matching_threshold);
break;
case 'w':
// write model to disk
writeLinemod(detector, filename);
printf("Wrote detector and templates to %s\n", filename.c_str());
break;
default:
;
}
}
return 0;
}
int
main (int argc,
char * argv[])
{
//cv::CommandLineParser parser(argc, argv, 1);
// Various settings and flags
bool show_match_result = true;
bool show_timings = false;
bool learn_online = false;
int num_classes = 0;
int matching_threshold = 60;
cv::Size roi_size (120, 60);
// int learning_lower_bound = 90;
// int learning_upper_bound = 95;
// Timers
Timer extract_timer;
Timer match_timer;
// Initialize HighGUI
help ();
cv::namedWindow ("color");
cv::namedWindow ("normals");
Mouse::start ("color");
//Debuging...
// /home/anh/catkin_ws/build/pre_grasp/linemod_templates.yml
// Initialize LINEMOD data structures
cv::Ptr<cv::linemod::Detector> detector;
std::string filename;
printf ("Loading linemod template......................... \n");
detector = readLinemod (argv[1]);
std::vector<std::string> ids = detector->classIds ();
num_classes = detector->numClasses ();
printf ("Loaded %s with %d classes and %d templates\n", argv[1], num_classes, detector->numTemplates ());
if (!ids.empty ())
{
printf ("Class ids:\n");
std::copy (ids.begin (), ids.end (), std::ostream_iterator<std::string> (std::cout, "\n"));
}
int num_modalities = (int) detector->getModalities ().size ();
printf ("Number of modalities: %d\n", num_modalities);
// Open Kinect sensor
cv::VideoCapture capture (CV_CAP_OPENNI);
if (!capture.isOpened ())
{
printf ("Could not open OpenNI-capable sensor\n");
return -1;
}
capture.set (CV_CAP_PROP_OPENNI_REGISTRATION, 1);
// double focal_length = capture.get (CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH);
//printf("Focal length = %f\n", focal_length);
// Main loop
// bool isFound = false;
cv::Mat color, depth;
for (;;)
{
// Capture next color/depth pair
capture.grab ();
capture.retrieve (depth, CV_CAP_OPENNI_DEPTH_MAP);
capture.retrieve (color, CV_CAP_OPENNI_BGR_IMAGE);
std::vector<cv::Mat> sources;
sources.push_back (color);
sources.push_back (depth);
cv::Mat display = color.clone ();
// Perform matching
std::vector<cv::linemod::Match> matches;
std::vector<std::string> class_ids;
std::vector<cv::Mat> quantized_images;
match_timer.start ();
detector->match (sources, (float) matching_threshold, matches, class_ids, quantized_images);
match_timer.stop ();
double focal_length = capture.get (CV_CAP_OPENNI_DEPTH_GENERATOR_FOCAL_LENGTH);
printf ("Focal length = %f\n", focal_length);
int classes_visited = 0;
std::set<std::string> visited;
printf ("Matches size: %d\n", (int) matches.size ());
for (int i = 0; (i < (int) matches.size ()) && (classes_visited < num_classes); ++i)
{
cv::linemod::Match m = matches[i];
if (visited.insert (m.class_id).second)
{
++classes_visited;
std::cout << "Visited : " << classes_visited << std::endl;
if (show_match_result)
{
printf ("Similarity: %5.1f%%; x: %3d; y: %3d; class: %s; template: %3d\n", m.similarity, m.x, m.y, m.class_id.c_str (), m.template_id);
}
// Draw matching template
const std::vector<cv::linemod::Template>& templates = detector->getTemplates (m.class_id, m.template_id);
drawResponse (templates, num_modalities, display, cv::Point (m.x, m.y), detector->getT (0));
}
}
if (show_match_result && matches.empty ())
printf ("No matches found...\n");
if (show_timings)
printf ("Matching: %.2fs\n", match_timer.time ());
cv::imshow ("color", display);
cv::imshow ("normals", quantized_images[1]);
// printf ("Waiting for input ... \n");
// getchar ();
cv::FileStorage fs;
char key = (char) cvWaitKey (10);
if (key == 'q' || key == 'n')
break;
switch (key)
{
case 'h':
help ();
break;
case 'm':
// toggle printing match result
show_match_result = !show_match_result;
printf ("Show match result %s\n", show_match_result ? "ON" : "OFF");
break;
case 't':
// toggle printing timings
show_timings = !show_timings;
printf ("Show timings %s\n", show_timings ? "ON" : "OFF");
break;
case 'l':
// toggle online learning
learn_online = !learn_online;
printf ("Online learning %s\n", learn_online ? "ON" : "OFF");
break;
case '[':
// decrement threshold
matching_threshold = std::max (matching_threshold - 1, -100);
printf ("New threshold: %d\n", matching_threshold);
break;
case ']':
// increment threshold
matching_threshold = std::min (matching_threshold + 1, +100);
printf ("New threshold: %d\n", matching_threshold);
break;
default:
;
}
}
return 0;
}
