Visual Servoing Platform  version 3.6.1 under development (2025-02-17)
tutorial-pose-from-points-image.cpp
1 
3 #include <visp3/core/vpConfig.h>
4 #include <visp3/core/vpIoTools.h>
5 #include <visp3/gui/vpDisplayFactory.h>
6 #include <visp3/io/vpImageIo.h>
7 
9 #include "pose_helper.h"
11 
12 int main(int, char *argv[])
13 {
14 #ifdef ENABLE_VISP_NAMESPACE
15  using namespace VISP_NAMESPACE_NAME;
16 #endif
17 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
18  std::shared_ptr<vpDisplay> display;
19 #else
20  vpDisplay *display = nullptr;
21 #endif
22  try {
25  vpImageIo::read(I, vpIoTools::getParent(argv[0]) + "/data/square.pgm");
27 
28 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
30 #else
32 #endif
33 
35  vpCameraParameters cam(840, 840, I.getWidth() / 2, I.getHeight() / 2);
38  std::vector<vpDot2> dot(4);
39  std::vector<vpImagePoint> ip(4);
40  dot[0].initTracking(I, vpImagePoint(193, 157));
41  dot[1].initTracking(I, vpImagePoint(203, 366));
42  dot[2].initTracking(I, vpImagePoint(313, 402));
43  dot[3].initTracking(I, vpImagePoint(304, 133));
46  std::vector<vpPoint> point;
47  point.push_back(vpPoint(-0.06, -0.06, 0));
48  point.push_back(vpPoint(0.06, -0.06, 0));
49  point.push_back(vpPoint(0.06, 0.06, 0));
50  point.push_back(vpPoint(-0.06, 0.06, 0));
52 
56  bool init = true;
57 
58  while (1) {
60  vpImageIo::read(I, vpIoTools::getParent(argv[0]) + "/data/square.pgm");
62  for (unsigned int i = 0; i < dot.size(); i++) {
63  dot[i].setGraphics(true);
64  dot[i].track(I);
65  ip[i] = dot[i].getCog();
66  }
69  computePose(point, ip, cam, init, cMo);
72  vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none);
75 
77  if (init)
78  init = false; // turn off pose initialisation
80 
82  if (vpDisplay::getClick(I, false))
83  break;
85 
87  vpTime::wait(40);
89  }
90  }
91  catch (const vpException &e) {
92  std::cout << "Catch an exception: " << e.getMessage() << std::endl;
93  }
94 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
95  if (display != nullptr) {
96  delete display;
97  }
98 #endif
99 }
Generic class defining intrinsic camera parameters.
static const vpColor none
Definition: vpColor.h:210
Class that defines generic functionalities for display.
Definition: vpDisplay.h:178
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
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 flush(const vpImage< unsigned char > &I)
error that can be emitted by ViSP classes.
Definition: vpException.h:60
const char * getMessage() const
Definition: vpException.cpp:65
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)
Definition: vpImageIo.cpp:147
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
unsigned int getWidth() const
Definition: vpImage.h:242
unsigned int getHeight() const
Definition: vpImage.h:181
static std::string getParent(const std::string &pathname)
Definition: vpIoTools.cpp:1314
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition: vpPoint.h:79
std::shared_ptr< vpDisplay > createDisplay()
Return a smart pointer vpDisplay specialization if a GUI library is available or nullptr otherwise.
vpDisplay * allocateDisplay()
Return a newly allocated vpDisplay specialization if a GUI library is available or nullptr otherwise.
VISP_EXPORT int wait(double t0, double t)