Visual Servoing Platform  version 3.1.0
tutorial-pose-from-points-tracking.cpp
#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_MODULE_SENSOR
#include <visp3/sensor/vp1394CMUGrabber.h>
#include <visp3/sensor/vp1394TwoGrabber.h>
#endif
#include <visp3/blob/vpDot2.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/vision/vpPose.h>
void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot, const vpCameraParameters &cam, bool init,
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
void track(vpImage<unsigned char> &I, std::vector<vpDot2> &dot, bool init);
#endif
void computePose(std::vector<vpPoint> &point, const std::vector<vpDot2> &dot, const vpCameraParameters &cam, bool init,
{
vpPose pose;
double x = 0, y = 0;
for (unsigned int i = 0; i < point.size(); i++) {
vpPixelMeterConversion::convertPoint(cam, dot[i].getCog(), x, y);
point[i].set_x(x);
point[i].set_y(y);
pose.addPoint(point[i]);
}
if (init == true) {
pose.computePose(vpPose::LAGRANGE, cMo_lag);
double residual_dem = pose.computeResidual(cMo_dem);
double residual_lag = pose.computeResidual(cMo_lag);
if (residual_dem < residual_lag)
cMo = cMo_dem;
else
cMo = cMo_lag;
}
}
#if defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)
void track(vpImage<unsigned char> &I, std::vector<vpDot2> &dot, bool init)
{
if (init) {
for (unsigned int i = 0; i < dot.size(); i++) {
dot[i].setGraphics(true);
dot[i].setGraphicsThickness(2);
dot[i].initTracking(I);
}
} else {
for (unsigned int i = 0; i < dot.size(); i++) {
dot[i].track(I);
}
}
}
#endif
int main()
{
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
(defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394) || (VISP_HAVE_OPENCV_VERSION >= 0x020100))
try {
#if defined(VISP_HAVE_DC1394)
g.open(I);
#elif defined(VISP_HAVE_CMU1394)
g.open(I);
#elif defined(VISP_HAVE_OPENCV)
cv::VideoCapture g(0); // open the default camera
if (!g.isOpened()) { // check if we succeeded
std::cout << "Failed to open the camera" << std::endl;
return -1;
}
cv::Mat frame;
g >> frame; // get a new frame from camera
#endif
// Parameters of our camera
vpCameraParameters cam(840, 840, I.getWidth() / 2, I.getHeight() / 2);
// The pose container
std::vector<vpDot2> dot(4);
std::vector<vpPoint> point;
double L = 0.06;
point.push_back(vpPoint(-L, -L, 0));
point.push_back(vpPoint(L, -L, 0));
point.push_back(vpPoint(L, L, 0));
point.push_back(vpPoint(-L, L, 0));
bool init = true;
#if defined(VISP_HAVE_X11)
vpDisplayX d(I);
#elif defined(VISP_HAVE_GDI)
#elif defined(VISP_HAVE_OPENCV)
#endif
while (1) {
// Image Acquisition
#if defined(VISP_HAVE_DC1394) || defined(VISP_HAVE_CMU1394)
g.acquire(I);
#elif defined(VISP_HAVE_OPENCV)
g >> frame;
#endif
track(I, dot, init);
computePose(point, dot, cam, init, cMo);
vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
if (init)
init = false; // turn off the initialisation specific stuff
if (vpDisplay::getClick(I, false))
break;
}
} catch (vpException &e) {
std::cout << "Catch an exception: " << e << std::endl;
}
#endif
}