void depthCallback(const sensor_msgs::ImageConstPtr& msg)
{
if (!LineMOD_Detector::got_color)
{
return;
}
cv_bridge::CvImagePtr depth_ptr;
bool show_match_result = true;
try
{
depth_ptr = cv_bridge::toCvCopy(msg, "");
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("depth cv_bridge exception: %s", e.what());
return;
}
LineMOD_Detector::sources.push_back(LineMOD_Detector::color_img);
LineMOD_Detector::sources.push_back(depth_ptr->image);
// Perform matching
std::vector<cv::linemod::Match> matches;
std::vector<cv::String> class_ids;
std::vector<cv::Mat> quantized_images;
LineMOD_Detector::detector->match(sources, (float)LineMOD_Detector::matching_threshold, matches, class_ids, quantized_images);
LineMOD_Detector::num_classes = detector->numClasses();
int classes_visited = 0;
std::set<std::string> visited;
for (int i = 0; (i < (int)matches.size()) && (classes_visited < LineMOD_Detector::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 = LineMOD_Detector::detector->getTemplates(m.class_id, m.template_id);
drawResponse(templates, LineMOD_Detector::num_modalities, LineMOD_Detector::display, cv::Point(m.x, m.y), LineMOD_Detector::detector->getT(0));
//std::cout << "pub_threshold " << LineMOD_Detector::pub_threshold << std::endl;
if (m.similarity > LineMOD_Detector::pub_threshold)
{
LineMOD_Detector::publishPoint(templates, m, depth_ptr->image, msg->header);
}
}
}
LineMOD_Detector::sources.clear();
}
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;
}
void disparityCallback(const stereo_msgs::DisparityImageConstPtr& msg)
{
ROS_DEBUG("Pub Threshold:%f ", LineMOD_Detector::pub_threshold);
if (!LineMOD_Detector::got_color)
{
return;
}
bool show_match_result = true;
cv_bridge::CvImagePtr disp_ptr;
try
{
disp_ptr = cv_bridge::toCvCopy(msg->image, "");
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("disp cv_bridge exception: %s", e.what());
return;
}
float f = msg->f;
float T = msg->T;
cv::Mat depth_img = (f*T*1000)/(disp_ptr->image).clone();
depth_img.convertTo(depth_img, CV_16U);
LineMOD_Detector::sources.push_back(LineMOD_Detector::color_img);
LineMOD_Detector::sources.push_back(depth_img);
// Perform matching
std::vector<cv::linemod::Match> matches;
std::vector<cv::String> class_ids;
std::vector<cv::Mat> quantized_images;
LineMOD_Detector::detector->match(sources, (float)LineMOD_Detector::matching_threshold, matches, class_ids, quantized_images);
LineMOD_Detector::num_classes = detector->numClasses();
ROS_DEBUG("Num Classes: %u", LineMOD_Detector::num_classes);
int classes_visited = 0;
std::set<std::string> visited;
ROS_DEBUG("Matches size: %u", (int)matches.size());
for (int i = 0; (i < (int)matches.size()) && (classes_visited < LineMOD_Detector::num_classes); ++i)
{
cv::linemod::Match m = matches[i];
ROS_DEBUG("Matching count: %u", i);
if (visited.insert(m.class_id).second)
{
++classes_visited;
if (show_match_result)
{
ROS_DEBUG("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);
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 = LineMOD_Detector::detector->getTemplates(m.class_id, m.template_id);
drawResponse(templates, LineMOD_Detector::num_modalities, LineMOD_Detector::display, cv::Point(m.x, m.y), LineMOD_Detector::detector->getT(0));
if (m.similarity > LineMOD_Detector::pub_threshold)
{
LineMOD_Detector::publishPoint(templates, m, depth_img, msg->header);
}
}
}
LineMOD_Detector::sources.clear();
}
int main(int argc, char **argv) {
// Define the display
size_t width = 640, height = 480;
// the model name can be specified on the command line.
std::string file_name(argv[1]), file_ext = file_name.substr(file_name.size() - 3, file_name.npos);
cv::Rect rect;
Renderer2d render(file_name, 0.14);
double focal_length_x = 525, focal_length_y = 525;
render.set_parameters(width, height, focal_length_x, focal_length_y);
cv::Ptr<cv::linemod::Detector> detector_ptr = cv::linemod::getDefaultLINE();
// Loop over a few views in front of the pattern
float xy_lim = 0.5;
for (float x = -xy_lim; x < xy_lim; x += 0.1)
for (float y = -xy_lim; y < xy_lim; y += 0.1)
for (float z = 0.6; z < 0.7; z += 0.1) {
cv::Vec3f up(0, z, -y);
up = up / norm(up);
// Rotate the vector
for (float theta = -10; theta < 20; theta += 10) {
cv::Vec3f Rvec(x, y, z);
Rvec = (theta * CV_PI / 180) * Rvec / norm(Rvec);
cv::Matx33f R;
cv::Rodrigues(Rvec, R);
cv::Vec3f up_rotated = R * up;
render.lookAt(0., y, z, up_rotated(0), up_rotated(1), up_rotated(2));
cv::Mat img, depth, mask;
render.render(img, depth, mask, rect);
std::vector<cv::Mat> sources(1);
sources[0] = img;
//sources[1] = depth;
detector_ptr->addTemplate(sources, "object1", mask);
// cv::imshow("img", img);
// cv::imshow("depth", depth);
// cv::imshow("mask", mask);
// cv::waitKey(0);
}
}
detector_ptr->writeClasses(file_name + std::string("_templates.yaml"));
cv::VideoCapture cap(0);
cv::Mat img;
int num_modalities = (int) detector_ptr->getModalities().size();
cv::namedWindow("result");
while (true) {
cap >> img;
std::vector<cv::Mat> sources(1, img);
std::vector<cv::linemod::Match> matches;
detector_ptr->match(sources, 93, matches);
for (size_t i = 0; i < matches.size(); ++i) {
const cv::linemod::Match & match = matches[i];
const std::vector<cv::linemod::Template>& templates = detector_ptr->getTemplates(match.class_id,
match.template_id);
drawResponse(templates, num_modalities, img, cv::Point(match.x, match.y), detector_ptr->getT(0));
};
cv::imshow("result", img);
cv::waitKey(5);
}
return 0;
}
