Using ALVisualCompass

<< return to C++ examples

The following example demonstrates how to use ALVisualCompass.

The whole example is available here alvisualcompass.zip.

The full documentation of this module can be found here.

Principle

This example enables you to choose your reference image, from which the deviation will be computed, and then displays the angle deviation between the current image and the reference one.

Run it with the robot’s IP adress as an argument.

The executable will display two windows, one for the reference image and one for the current one taken by ALVisualCompass. The current deviation will by displayed in the “Current image” window, in degrees: Wz is the rotation along the vertical Z axis, and Wy is the rotation along the horizontal Y axis.

To use the example:

• Press r to start the compass. The reference image will be the current image at startup.
• Press p to pause / unpause the compass.
• Press s to stop the compass (for example to set another reference).
• Press Esc to exit the program. Use this instead of interrupting the program to unsubscribe to the compass properly.

Source file

alvisualcompass_example.cpp

/**
 *
 * This example demonstrates how to use ALVisualCompass.
 *
 * Copyright Aldebaran Robotics
 */

#include <iostream>

#include <alerror/alerror.h>
#include <alcommon/albroker.h>
#include <alproxies/alvideodeviceproxy.h>
#include <alproxies/alvisualcompassproxy.h>
#include <alproxies/almemoryproxy.h>
#include <alvision/alvisiondefinitions.h>

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>


using namespace AL;

int main(int argc, char* argv[]) {
  if (argc != 2) {
    std::cerr << "Usage: alvisualcompass_example robotIp" << std::endl;
    return 1;
  }

  boost::shared_ptr<AL::ALBroker> broker = AL::ALBroker::createBroker(
        "broker", "", 0, argv[1], 9559, 0);

  boost::shared_ptr<ALVideoDeviceProxy> videoProxy;
  boost::shared_ptr<ALVisualCompassProxy> compassProxy;
  boost::shared_ptr<ALMemoryProxy> memoryProxy;
  try {
    // Creating proxy to ALVisualCompass.
    videoProxy = boost::shared_ptr<ALVideoDeviceProxy>(new ALVideoDeviceProxy(broker));
    compassProxy = boost::shared_ptr<ALVisualCompassProxy>(new ALVisualCompassProxy(broker));
    memoryProxy = boost::shared_ptr<ALMemoryProxy>(new ALMemoryProxy(broker));
  }
  catch (const ALError& e) {
    std::cerr << "Could not create proxies: " << e.what() << std::endl;
    return 2;
  }

  // Configure VideoDevice
  std::string videoName =
      videoProxy->subscribeCamera(argv[0], AL::kTopCamera, AL::kQVGA, AL::kYuvColorSpace, 15);

  // Reference will be set each time the module is subscribed.
  compassProxy->enableReferenceRefresh(true);
  // Image resolution is QVGA (320x240).
  compassProxy->setResolution(kQVGA);

  // Initialize image containers for display.
  cv::Mat referenceImage = cv::Mat::zeros(240, 320, CV_8UC1);
  ALValue refImage, curImage;
  cv::Mat currentImage = cv::Mat::zeros(240, 320, CV_8UC1);
  cv::Mat matchImage = cv::Mat::zeros(480, 320, CV_8UC1);

  ALValue deviation;
  ALValue matchInfo;
  bool running=false;
  bool pauseStatus = false;
  cv::Mat roi;
  // Loop displaying the images.
  while(true) {
    char key = cv::waitKey(50);
    // Esc key to exit.
    if (key == 27) {
      break;
    }

    switch (key) {
      // 'p' key to toggle pause status.
      case 'p':
        // Set the reference.
        pauseStatus = !pauseStatus;
        compassProxy->pause(pauseStatus);
        break;

        // Enter to launch the processing.
      case 'r':
        // Subscribe to launch the processing.
        // Get the reference image for display.
        compassProxy->subscribe("VisualCompassTest");
        refImage = compassProxy->getReferenceImage();
        referenceImage.data = (uchar*) refImage[6].GetBinary();
        running = true;
        break;

        // 's' key to stop the processing.
      case 's':
        // Unsubscribe if the module is running.
        if (running) {
          compassProxy->unsubscribe("VisualCompassTest");
          running = false;
        }
        break;

      default:
        break;
    }
    if (running) {
      // Retrieve the current image for display.
      curImage = videoProxy->getImageRemote(videoName);
      currentImage.data = (uchar*) curImage[6].GetBinary();

      // Get the deviation information from the ALMemory event.
      try {
        deviation = memoryProxy->getData("VisualCompass/Deviation");
        matchInfo = memoryProxy->getData("VisualCompass/Match");
      }
      catch (const ALError&) {
        continue;
      }

      if (deviation.getSize() <= 0) {
        continue;
      }

      float wy = deviation[0][0], wz = deviation[0][1];
      // Convert it to degrees.
      wz = wz * 180.0f / 3.14f;
      wy = wy * 180.0f / 3.14f;
      // Display it on the image.
      char buffer [100];
      sprintf(buffer, "Wz: %.2f deg, Wy: %.2f deg", wz, wy);
      cv::putText(currentImage, buffer, cv::Point(10,30), CV_FONT_HERSHEY_PLAIN,
                  1.0f, cv::Scalar(255), 2);

      roi = matchImage(cv::Rect(0,0, 320, 240));
      referenceImage.copyTo(roi);
      roi = matchImage(cv::Rect(0, 240, 320, 240));
      currentImage.copyTo(roi);

      if (matchInfo.getSize() <= 0) {
        continue;
      }

      try {
        for (int i = 0; i < static_cast<int>(matchInfo[3][1].getSize()); ++i) {
          ALValue match = matchInfo[2][matchInfo[3][1][i]];
          ALValue refKp = matchInfo[0][match[0]];
          cv::Point ref = cv::Point((float) refKp[0][0], (float) refKp[0][1]);
          ALValue curKp = matchInfo[1][match[1]];
          cv::Point cur = cv::Point((float) curKp[0][0], (float) curKp[0][1]);
          cur.y += 240;
          cv::line(matchImage, cur, ref, cv::Scalar(255));
          cv::circle(matchImage, ref, (float) refKp[1], cv::Scalar(255));
          cv::circle(matchImage, cur, (float) curKp[1], cv::Scalar(255));
        }
      }
      catch (const ALError&) {
        continue;
      }
    }
    cv::imshow("Reference image", referenceImage);
    cv::imshow("Current image", currentImage);
    cv::imshow("Match", matchImage);
  }
  // Unsubscribe if the module is running.
  if (running)
  {
    videoProxy->unsubscribe(videoName);
    compassProxy->unsubscribe("VisualCompassTest");
  }
  return 0;
}

CMakeLists.txt:

The corresponding CMakeLists.txt file is the following:

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)
project(alvisualcompass_example)
find_package(qibuild)

qi_create_bin(alvisualcompass_example alvisualcompass_example.cpp)
qi_use_lib(alvisualcompass_example OPENCV2_CORE OPENCV2_HIGHGUI ALCOMMON ALPROXIES ALVISION)