Tutorial: Real-time curves plotter tool

This tutorial focuses on real-time curves drawing. It shows how to modify tutorial-ibvs-4pts.cpp introduced in Tutorial: Image-based visual servo to draw the evolution of the visual features error and the camera velocity skew vector during an image-based visual servoing.

Note that all the material (source code and image) described in this tutorial is part of ViSP source code and could be downloaded using the following command:

$ svn export https://github.com/lagadic/visp.git/trunk/tutorial/visual-servoing/ibvs

The modified code also available in tutorial-ibvs-4pts-plotter.cpp is the following.

#include <visp3/gui/vpPlot.h>
#include <visp3/robot/vpSimulatorCamera.h>
#include <visp3/visual_features/vpFeatureBuilder.h>
#include <visp3/vs/vpServo.h>

int main()
{
  try {
    vpHomogeneousMatrix cdMo(0, 0, 0.75, 0, 0, 0);
    vpHomogeneousMatrix cMo(0.15, -0.1, 1., vpMath::rad(10), vpMath::rad(-10), vpMath::rad(50));

    vpPoint point[4];
    point[0].setWorldCoordinates(-0.1, -0.1, 0);
    point[1].setWorldCoordinates(0.1, -0.1, 0);
    point[2].setWorldCoordinates(0.1, 0.1, 0);
    point[3].setWorldCoordinates(-0.1, 0.1, 0);

    vpServo task;
    task.setServo(vpServo::EYEINHAND_CAMERA);
    task.setInteractionMatrixType(vpServo::CURRENT);
    task.setLambda(0.5);

    vpFeaturePoint p[4], pd[4];
    for (unsigned int i = 0; i < 4; i++) {
      point[i].track(cdMo);
      vpFeatureBuilder::create(pd[i], point[i]);
      point[i].track(cMo);
      vpFeatureBuilder::create(p[i], point[i]);
      task.addFeature(p[i], pd[i]);
    }

    vpHomogeneousMatrix wMc, wMo;
    vpSimulatorCamera robot;
    robot.setSamplingTime(0.040);
    robot.getPosition(wMc);
    wMo = wMc * cMo;

#ifdef VISP_HAVE_DISPLAY
    vpPlot plotter(2, 250 * 2, 500, 100, 200, "Real time curves plotter");
    plotter.setTitle(0, "Visual features error");
    plotter.setTitle(1, "Camera velocities");

    plotter.initGraph(0, 8);
    plotter.initGraph(1, 6);

    plotter.setLegend(0, 0, "x1");
    plotter.setLegend(0, 1, "y1");
    plotter.setLegend(0, 2, "x2");
    plotter.setLegend(0, 3, "y2");
    plotter.setLegend(0, 4, "x3");
    plotter.setLegend(0, 5, "y3");
    plotter.setLegend(0, 6, "x4");
    plotter.setLegend(0, 7, "y4");

    plotter.setLegend(1, 0, "v_x");
    plotter.setLegend(1, 1, "v_y");
    plotter.setLegend(1, 2, "v_z");
    plotter.setLegend(1, 3, "w_x");
    plotter.setLegend(1, 4, "w_y");
    plotter.setLegend(1, 5, "w_z");
#endif

    unsigned int iter = 0;
    while (1) {
      robot.getPosition(wMc);
      cMo = wMc.inverse() * wMo;
      for (unsigned int i = 0; i < 4; i++) {
        point[i].track(cMo);
        vpFeatureBuilder::create(p[i], point[i]);
      }
      vpColVector v = task.computeControlLaw();
      robot.setVelocity(vpRobot::CAMERA_FRAME, v);

#ifdef VISP_HAVE_DISPLAY
      plotter.plot(0, iter, task.getError());
      plotter.plot(1, iter, v);
#endif
      if ((task.getError()).sumSquare() < 0.0001)
        break;

      iter++;
    }
    std::cout << "Convergence in " << iter << " iterations" << std::endl;

    task.kill();

#ifdef VISP_HAVE_DISPLAY
    plotter.saveData(0, "error.dat");
    plotter.saveData(1, "vc.dat");

    vpDisplay::getClick(plotter.I);
#endif
  } catch (const vpException &e) {
    std::cout << "Catch an exception: " << e << std::endl;
  }
}

The last image of the drawing is the following:

Last image produced by the plotter.

Now we describe the new lines that were introduced:

#include <visp3/core/vpPlot.h>

Include the header of the vpPlot class that allows curves drawing.

#ifdef VISP_HAVE_DISPLAY
  vpPlot plotter(2, 250*2, 500, 100, 200, "Real time curves plotter");

Since the plotter opens a windows to display the graphics, the usage of vpPlot class is only possible if ViSP is build with a 3rd party that allows display capabilities; either libx11, GDI, OpenCV or GTK. If this is the case, we create an instance of the vpPlot class. The window that is created will contain two graphics. The windows size will be 500 by 500 pixels. The window position will be (100, 200), and the title "Real time curves plotter".

plotter.setTitle(0, "Visual features error");
plotter.setTitle(1, "Camera velocities");

To differentiate the graphics we associate a title to each of them. The first graphic (the one with index 0) will be designed to draw the evolution of the visual features error, while the second (index 1) will be designed to draw the camera velocities.

plotter.initGraph(0, 8);
plotter.initGraph(1, 6);

Here we initialize the first graphic to be able to plot 8 curves. We recall that we have 4 points, each point has 2 visual features (x and y), that is why there are 8 curves to plot. The second graphic is designed to plot 6 curves corresponding to the camera velocities (3 translation velocities in m/s and 3 rotation velocities in rad/s).

  plotter.setLegend(0, 0, "x1");
  plotter.setLegend(0, 1, "y1");
  plotter.setLegend(0, 2, "x2");
  plotter.setLegend(0, 3, "y2");
  plotter.setLegend(0, 4, "x3");
  plotter.setLegend(0, 5, "y3");
  plotter.setLegend(0, 6, "x4");
  plotter.setLegend(0, 7, "y4");

  plotter.setLegend(1, 0, "v_x");
  plotter.setLegend(1, 1, "v_y");
  plotter.setLegend(1, 2, "v_z");
  plotter.setLegend(1, 3, "w_x");
  plotter.setLegend(1, 4, "w_y");
  plotter.setLegend(1, 5, "w_z");
#endif

The previous lines allow to associate a legend to each curve.

#ifdef VISP_HAVE_DISPLAY
    plotter.plot(0, iter, task.getError());
    plotter.plot(1, iter, v);
#endif

Once the plotter is initialized, in the servo loop we add at each iteration the corresponding values of the visual features error , and the camera velocities .

#ifdef VISP_HAVE_DISPLAY
  plotter.saveData(0, "error.dat");
  plotter.saveData(1, "vc.dat");
#endif

At the end of the servo loop, we save the data that were plotted in two separate files, one for each graphic. The first line of each text file is the graphic title. Then the coordinates along x,y and z are given in separated columns for each data.

vpDisplay::getClick(plotter.I);

Before exiting the program we wait for a human mouse click in the plotter window.