Visual Servoing Platform  version 3.6.1 under development (2025-02-18)
tutorial-ibvs-4pts-wireframe-camera.cpp
1 
2 #include <visp3/core/vpConfig.h>
3 #include <visp3/gui/vpDisplayFactory.h>
4 #include <visp3/gui/vpProjectionDisplay.h>
5 #include <visp3/robot/vpSimulatorCamera.h>
6 #include <visp3/robot/vpWireFrameSimulator.h>
7 #include <visp3/visual_features/vpFeatureBuilder.h>
8 #include <visp3/vs/vpServo.h>
9 #include <visp3/vs/vpServoDisplay.h>
10 
11 #ifdef ENABLE_VISP_NAMESPACE
12 using namespace VISP_NAMESPACE_NAME;
13 #endif
14 
15 void display_trajectory(const vpImage<unsigned char> &I, std::vector<vpPoint> &point, const vpHomogeneousMatrix &cMo,
16  const vpCameraParameters &cam)
17 {
18  static std::vector<vpImagePoint> traj[4];
19  vpImagePoint cog;
20  for (unsigned int i = 0; i < 4; i++) {
21  // Project the point at the given camera position
22  point[i].project(cMo);
23  vpMeterPixelConversion::convertPoint(cam, point[i].get_x(), point[i].get_y(), cog);
24  traj[i].push_back(cog);
25  }
26  for (unsigned int i = 0; i < 4; i++) {
27  for (unsigned int j = 1; j < traj[i].size(); j++) {
28  vpDisplay::displayLine(I, traj[i][j - 1], traj[i][j], vpColor::green);
29  }
30  }
31 }
32 
33 int main()
34 {
35 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
36  std::shared_ptr<vpDisplay> displayInt;
37  std::shared_ptr<vpDisplay> displayExt;
38 #else
39  vpDisplay *displayInt = nullptr;
40  vpDisplay *displayExt = nullptr;
41 #endif
42  try {
43  vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
44  vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));
45 
46  std::vector<vpPoint> point(4);
47  point[0].setWorldCoordinates(-0.1, -0.1, 0);
48  point[1].setWorldCoordinates(0.1, -0.1, 0);
49  point[2].setWorldCoordinates(0.1, 0.1, 0);
50  point[3].setWorldCoordinates(-0.1, 0.1, 0);
51 
52  vpServo task;
55  task.setLambda(0.5);
56 
57  vpFeaturePoint p[4], pd[4];
58  for (unsigned int i = 0; i < 4; i++) {
59  point[i].track(cdMo);
60  vpFeatureBuilder::create(pd[i], point[i]);
61  point[i].track(cMo);
62  vpFeatureBuilder::create(p[i], point[i]);
63  task.addFeature(p[i], pd[i]);
64  }
65 
66  vpHomogeneousMatrix wMc, wMo;
67  vpSimulatorCamera robot;
68  robot.setSamplingTime(0.040);
69  robot.getPosition(wMc);
70  wMo = wMc * cMo;
71 
72  vpImage<unsigned char> Iint(480, 640, 0);
73  vpImage<unsigned char> Iext(480, 640, 0);
74 #if defined(VISP_HAVE_DISPLAY)
75 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
76  displayInt = vpDisplayFactory::createDisplay(Iint, 0, 0, "Internal view");
77  displayExt = vpDisplayFactory::createDisplay(Iext, 670, 0, "External view");
78 #else
79  displayInt = vpDisplayFactory::allocateDisplay(Iint, 0, 0, "Internal view");
80  displayExt = vpDisplayFactory::allocateDisplay(Iext, 670, 0, "External view");
81 #endif
82 #else
83  std::cout << "No image viewer is available..." << std::endl;
84 #endif
85 
86  vpCameraParameters cam(840, 840, Iint.getWidth() / 2, Iint.getHeight() / 2);
87  vpHomogeneousMatrix cextMo(0, 0, 3, 0, 0, 0);
88 
91  sim.setCameraPositionRelObj(cMo);
92  sim.setDesiredCameraPosition(cdMo);
93  sim.setExternalCameraPosition(cextMo);
96 
97  while (1) {
98  robot.getPosition(wMc);
99  cMo = wMc.inverse() * wMo;
100  for (unsigned int i = 0; i < 4; i++) {
101  point[i].track(cMo);
102  vpFeatureBuilder::create(p[i], point[i]);
103  }
104  vpColVector v = task.computeControlLaw();
106 
107  sim.setCameraPositionRelObj(cMo);
108 
109  vpDisplay::display(Iint);
110  vpDisplay::display(Iext);
111 
112  sim.getInternalImage(Iint);
113  sim.getExternalImage(Iext);
114 
115  display_trajectory(Iint, point, cMo, cam);
116  vpDisplay::flush(Iint);
117  vpDisplay::flush(Iext);
118 
119  // A click in the internal view to exit
120  if (vpDisplay::getClick(Iint, false))
121  break;
122  vpTime::wait(1000 * robot.getSamplingTime());
123  }
124  }
125  catch (const vpException &e) {
126  std::cout << "Catch an exception: " << e << std::endl;
127  }
128 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
129  if (displayInt != nullptr) {
130  delete displayInt;
131  }
132  if (displayExt != nullptr) {
133  delete displayExt;
134  }
135 #endif
136 }
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
static const vpColor green
Definition: vpColor.h:201
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 displayLine(const vpImage< unsigned char > &I, const vpImagePoint &ip1, const vpImagePoint &ip2, const vpColor &color, unsigned int thickness=1, bool segment=true)
static void flush(const vpImage< unsigned char > &I)
error that can be emitted by ViSP classes.
Definition: vpException.h:60
static void create(vpFeaturePoint &s, const vpCameraParameters &cam, const vpImagePoint &t)
Class that defines a 2D point visual feature which is composed by two parameters that are the cartes...
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpHomogeneousMatrix inverse() const
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
static double rad(double deg)
Definition: vpMath.h:129
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
void setVelocity(const vpRobot::vpControlFrameType frame, const vpColVector &vel) VP_OVERRIDE
@ CAMERA_FRAME
Definition: vpRobot.h:84
void setInteractionMatrixType(const vpServoIteractionMatrixType &interactionMatrixType, const vpServoInversionType &interactionMatrixInversion=PSEUDO_INVERSE)
Definition: vpServo.cpp:380
@ EYEINHAND_CAMERA
Definition: vpServo.h:161
void addFeature(vpBasicFeature &s_cur, vpBasicFeature &s_star, unsigned int select=vpBasicFeature::FEATURE_ALL)
Definition: vpServo.cpp:331
void setLambda(double c)
Definition: vpServo.h:991
void setServo(const vpServoType &servo_type)
Definition: vpServo.cpp:134
vpColVector computeControlLaw()
Definition: vpServo.cpp:705
@ CURRENT
Definition: vpServo.h:202
Class that defines the simplest robot: a free flying camera.
Implementation of a wire frame simulator. Compared to the vpSimulator class, it does not require thir...
void setCameraPositionRelObj(const vpHomogeneousMatrix &cMo_)
void getInternalImage(vpImage< unsigned char > &I)
void initScene(const vpSceneObject &obj, const vpSceneDesiredObject &desiredObject)
void setExternalCameraPosition(const vpHomogeneousMatrix &cam_Mf)
void setDesiredCameraPosition(const vpHomogeneousMatrix &cdMo_)
void setInternalCameraParameters(const vpCameraParameters &cam)
void setExternalCameraParameters(const vpCameraParameters &cam)
void getExternalImage(vpImage< unsigned char > &I)
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)