ArUco markers for docking and alignment

Worked example

Read it before you run it

auto dict = cv::aruco::getPredefinedDictionary(cv::aruco::DICT_4X4_50);
// Choose the marker family: 4×4 bits, 50 IDs.
std::vector<int> ids; std::vector<std::vector<cv::Point2f>> corners;
// Outputs: which markers, and where their corners are in the image.
cv::aruco::detectMarkers(frame, dict, corners, ids);
// Find markers in the image.
std::vector<cv::Vec3d> rvecs, tvecs;
// Rotation and translation of each marker relative to the camera.
cv::aruco::estimatePoseSingleMarkers(corners, 0.10f, K, dist, rvecs, tvecs);
// Estimate poses for 10 cm markers using camera intrinsics K and distortion dist.
double w_cmd = 1.0 * std::atan2(tvecs[0][0], tvecs[0][2]);
// Steer toward the marker with gain 1.0 (x = sideways, z = forward).

Practice code

Now type it yourself

Copy the starter into practice.cpp and replace every TODO with your own code. Build and run it: g++ -std=c++17 -Wall -Wextra -Wpedantic practice.cpp -o practice -pthread && ./practice

#include <cmath>
#include <iostream>

int main() {
  const double marker_x = 0.5, marker_y = 0.1;  // marker in base_link: 0.5 m ahead, 0.1 m to the left
  const double gain = 1.0;
  // TODO: angular velocity command = gain * atan2(marker_y, marker_x)
  std::cout << w << '\n';
  return 0;
}

0.197396

#include <cmath>
#include <iostream>

int main() {
  const double marker_x = 0.5, marker_y = 0.1;  // marker in base_link: 0.5 m ahead, 0.1 m to the left
  const double gain = 1.0;
  const double w = gain * std::atan2(marker_y, marker_x);
  std::cout << w << '\n';
  return 0;
}