Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-detection-object-mbt2.cpp
#include <visp3/core/vpConfig.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpVideoReader.h>
#include <visp3/mbt/vpMbGenericTracker.h>
#include <visp3/vision/vpKeyPoint.h>
#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_FEATURES2D)
void learnCube(const vpImage<unsigned char> &I, vpMbGenericTracker &tracker, vpKeyPoint &keypoint_learning, int id)
{
std::vector<cv::KeyPoint> trainKeyPoints;
double elapsedTime;
keypoint_learning.detect(I, trainKeyPoints, elapsedTime);
std::vector<vpPolygon> polygons;
std::vector<std::vector<vpPoint> > roisPt;
std::pair<std::vector<vpPolygon>, std::vector<std::vector<vpPoint> > > pair = tracker.getPolygonFaces();
polygons = pair.first;
roisPt = pair.second;
std::vector<cv::Point3f> points3f;
tracker.getPose(cMo);
tracker.getCameraParameters(cam);
vpKeyPoint::compute3DForPointsInPolygons(cMo, cam, trainKeyPoints, polygons, roisPt, points3f);
keypoint_learning.buildReference(I, trainKeyPoints, points3f, true, id);
for (std::vector<cv::KeyPoint>::const_iterator it = trainKeyPoints.begin(); it != trainKeyPoints.end(); ++it) {
vpDisplay::displayCross(I, (int)it->pt.y, (int)it->pt.x, 4, vpColor::red);
}
}
#endif
int main(int argc, char **argv)
{
#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_IMGPROC) && defined(HAVE_OPENCV_FEATURES2D)
try {
std::string videoname = "cube.mp4";
for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--name")
videoname = std::string(argv[i + 1]);
else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
std::cout << "\nUsage: " << argv[0] << " [--name <video name>] [--help] [-h]\n" << std::endl;
return EXIT_SUCCESS;
}
}
std::string parentname = vpIoTools::getParent(videoname);
std::string objectname = vpIoTools::getNameWE(videoname);
if (!parentname.empty())
objectname = parentname + "/" + objectname;
std::cout << "Video name: " << videoname << std::endl;
std::cout << "Tracker requested config files: " << objectname << ".[init,"
<< "xml,"
<< "cao or wrl]" << std::endl;
std::cout << "Tracker optional config files: " << objectname << ".[ppm]" << std::endl;
bool usexml = false;
if (vpIoTools::checkFilename(objectname + ".xml")) {
tracker.loadConfigFile(objectname + ".xml");
tracker.getCameraParameters(cam);
usexml = true;
}
if (!usexml) {
vpMe me;
me.setMaskSize(5);
me.setMaskNumber(180);
me.setRange(7);
me.setThreshold(20);
me.setMu1(0.5);
me.setMu2(0.5);
tracker.setMovingEdge(me);
cam.initPersProjWithoutDistortion(547, 542, 339, 235);
tracker.setCameraParameters(cam);
tracker.setAngleAppear(vpMath::rad(89));
tracker.setAngleDisappear(vpMath::rad(89));
tracker.setNearClippingDistance(0.01);
tracker.setFarClippingDistance(10.0);
tracker.setClipping(tracker.getClipping() | vpMbtPolygon::FOV_CLIPPING);
}
tracker.setOgreVisibilityTest(false);
if (vpIoTools::checkFilename(objectname + ".cao"))
tracker.loadModel(objectname + ".cao");
else if (vpIoTools::checkFilename(objectname + ".wrl"))
tracker.loadModel(objectname + ".wrl");
tracker.setDisplayFeatures(true);
vpKeyPoint keypoint_learning("ORB", "ORB", "BruteForce-Hamming");
#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
keypoint_learning.setDetectorParameter("ORB", "nLevels", 1);
#else
cv::Ptr<cv::ORB> orb_learning = keypoint_learning.getDetector("ORB").dynamicCast<cv::ORB>();
if (orb_learning) {
orb_learning->setNLevels(1);
}
#endif
#if defined(VISP_HAVE_X11)
vpDisplayX display;
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI display;
#elif defined(HAVE_OPENCV_HIGHGUI)
vpDisplayOpenCV display;
#else
std::cout << "No image viewer is available..." << std::endl;
return EXIT_FAILURE;
#endif
/*
* Start the part of the code dedicated to object learning from 3 images
*/
std::string imageName [] = { "cube0001.png", "cube0150.png", "cube0200.png" };
vpHomogeneousMatrix initPoseTab [] = {
vpHomogeneousMatrix(0.02143385294, 0.1098083886, 0.5127439561, 2.087159614, 1.141775176, -0.4701291124),
vpHomogeneousMatrix(0.02651282185, -0.03713587374, 0.6873765919, 2.314744454, 0.3492296488, -0.1226054828),
vpHomogeneousMatrix(0.02965448956, -0.07283091786, 0.7253526051, 2.300529617, -0.4286674806, 0.1788761025) };
for (int i = 0; i < 3; i++) {
vpImageIo::read(I, imageName[i]);
if (i == 0) {
display.init(I, 10, 10);
}
std::stringstream title;
title << "Learning cube on image: " << imageName[i];
vpDisplay::setTitle(I, title.str().c_str());
tracker.setPose(I, initPoseTab[i]);
tracker.track(I);
tracker.getPose(cMo);
tracker.display(I, cMo, cam, vpColor::red);
learnCube(I, tracker, keypoint_learning, i);
vpDisplay::displayText(I, 10, 10, "Learning step: keypoints are detected on visible cube faces", vpColor::red);
if (i < 2) {
vpDisplay::displayText(I, 30, 10, "Click to continue the learning...", vpColor::red);
}
else {
vpDisplay::displayText(I, 30, 10, "Click to continue with the detection...", vpColor::red);
}
}
keypoint_learning.saveLearningData("cube_learning_data.bin", true);
/*
* Start the part of the code dedicated to detection and localization
*/
vpKeyPoint keypoint_detection("ORB", "ORB", "BruteForce-Hamming");
#if (VISP_HAVE_OPENCV_VERSION < 0x030000)
keypoint_detection.setDetectorParameter("ORB", "nLevels", 1);
#else
cv::Ptr<cv::ORB> orb_detector = keypoint_detection.getDetector("ORB").dynamicCast<cv::ORB>();
orb_detector = keypoint_detection.getDetector("ORB").dynamicCast<cv::ORB>();
if (orb_detector) {
orb_detector->setNLevels(1);
}
#endif
keypoint_detection.loadLearningData("cube_learning_data.bin", true);
keypoint_detection.createImageMatching(I, IMatching);
g.setFileName(videoname);
g.open(I);
#if defined(VISP_HAVE_X11)
vpDisplayX display2;
#elif defined(VISP_HAVE_GTK)
vpDisplayGTK display2;
#elif defined(VISP_HAVE_GDI)
vpDisplayGDI display2;
#elif defined(HAVE_OPENCV_HIGHGUI)
vpDisplayOpenCV display2;
#endif
display2.init(IMatching, 50, 50, "Display matching between learned and current images");
vpDisplay::setTitle(I, "Cube detection and localization");
double error;
bool click_done = false;
while (!g.end()) {
g.acquire(I);
keypoint_detection.insertImageMatching(I, IMatching);
vpDisplay::display(IMatching);
vpDisplay::displayText(I, 10, 10, "Detection and localization in process...", vpColor::red);
double elapsedTime;
if (keypoint_detection.matchPoint(I, cam, cMo, error, elapsedTime)) {
tracker.setPose(I, cMo);
tracker.display(I, cMo, cam, vpColor::red, 2);
vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
keypoint_detection.displayMatching(I, IMatching);
std::vector<vpImagePoint> ransacInliers = keypoint_detection.getRansacInliers();
std::vector<vpImagePoint> ransacOutliers = keypoint_detection.getRansacOutliers();
for (std::vector<vpImagePoint>::const_iterator it = ransacInliers.begin(); it != ransacInliers.end(); ++it) {
vpImagePoint imPt(*it);
imPt.set_u(imPt.get_u() + I.getWidth());
imPt.set_v(imPt.get_v() + I.getHeight());
}
for (std::vector<vpImagePoint>::const_iterator it = ransacOutliers.begin(); it != ransacOutliers.end(); ++it) {
vpImagePoint imPt(*it);
imPt.set_u(imPt.get_u() + I.getWidth());
imPt.set_v(imPt.get_v() + I.getHeight());
vpDisplay::displayCircle(IMatching, imPt, 4, vpColor::red);
}
keypoint_detection.displayMatching(I, IMatching);
cam2.initPersProjWithoutDistortion(cam.get_px(), cam.get_py(), cam.get_u0() + I.getWidth(),
cam.get_v0() + I.getHeight());
tracker.setCameraParameters(cam2);
tracker.setPose(IMatching, cMo);
tracker.display(IMatching, cMo, cam2, vpColor::red, 2);
vpDisplay::displayFrame(IMatching, cMo, cam2, 0.05, vpColor::none, 3);
}
vpDisplay::displayText(IMatching, 30, 10, "A click to exit.", vpColor::red);
vpDisplay::flush(IMatching);
if (vpDisplay::getClick(I, false)) {
click_done = true;
break;
}
if (vpDisplay::getClick(IMatching, false)) {
click_done = true;
break;
}
}
if (!click_done)
vpDisplay::getClick(IMatching);
}
catch (const vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#else
(void)argc;
(void)argv;
std::cout << "Install OpenCV and rebuild ViSP to use this example." << std::endl;
#endif
return EXIT_SUCCESS;
}
Generic class defining intrinsic camera parameters.
void initPersProjWithoutDistortion(double px, double py, double u0, double v0)
static const vpColor red
Definition vpColor.h:211
static const vpColor none
Definition vpColor.h:223
static const vpColor green
Definition vpColor.h:214
Display for windows using GDI (available on any windows 32 platform).
The vpDisplayGTK allows to display image using the GTK 3rd party library. Thus to enable this class G...
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition vpDisplayX.h:132
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void displayCircle(const vpImage< unsigned char > &I, const vpImageCircle &circle, const vpColor &color, bool fill=false, unsigned int thickness=1)
static void display(const vpImage< unsigned char > &I)
static void displayFrame(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, double size, const vpColor &color=vpColor::none, unsigned int thickness=1, const vpImagePoint &offset=vpImagePoint(0, 0), const std::string &frameName="", const vpColor &textColor=vpColor::black, const vpImagePoint &textOffset=vpImagePoint(15, 15))
static void displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void setTitle(const vpImage< unsigned char > &I, const std::string &windowtitle)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
Definition vpException.h:59
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition of the vpImage class member functions.
Definition vpImage.h:135
unsigned int getWidth() const
Definition vpImage.h:242
unsigned int getHeight() const
Definition vpImage.h:184
static bool checkFilename(const std::string &filename)
static std::string getNameWE(const std::string &pathname)
static std::string getParent(const std::string &pathname)
Class that allows keypoints detection (and descriptors extraction) and matching thanks to OpenCV libr...
Definition vpKeyPoint.h:212
void detect(const vpImage< unsigned char > &I, std::vector< cv::KeyPoint > &keyPoints, const vpRect &rectangle=vpRect())
static void compute3DForPointsInPolygons(const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, std::vector< cv::KeyPoint > &candidates, const std::vector< vpPolygon > &polygons, const std::vector< std::vector< vpPoint > > &roisPt, std::vector< cv::Point3f > &points, cv::Mat *descriptors=NULL)
unsigned int buildReference(const vpImage< unsigned char > &I)
static double rad(double deg)
Definition vpMath.h:116
Real-time 6D object pose tracking using its CAD model.
virtual void getPose(vpHomogeneousMatrix &cMo) const
virtual void getCameraParameters(vpCameraParameters &camera) const
virtual std::pair< std::vector< vpPolygon >, std::vector< std::vector< vpPoint > > > getPolygonFaces(bool orderPolygons=true, bool useVisibility=true, bool clipPolygon=false)
Definition vpMe.h:122
void setMu1(const double &mu_1)
Definition vpMe.h:353
void setSampleStep(const double &s)
Definition vpMe.h:390
void setRange(const unsigned int &r)
Definition vpMe.h:383
void setLikelihoodThresholdType(const vpLikelihoodThresholdType likelihood_threshold_type)
Definition vpMe.h:445
void setMaskSize(const unsigned int &a)
Definition vpMe.cpp:452
void setNbTotalSample(const int &nb)
Definition vpMe.h:367
void setMu2(const double &mu_2)
Definition vpMe.h:360
@ NORMALIZED_THRESHOLD
Easy-to-use normalized likelihood threshold corresponding to the minimal luminance contrast to consid...
Definition vpMe.h:132
void setMaskNumber(const unsigned int &a)
Definition vpMe.cpp:445
void setThreshold(const double &t)
Definition vpMe.h:435
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
void acquire(vpImage< vpRGBa > &I)
void open(vpImage< vpRGBa > &I)
void setFileName(const std::string &filename)