Visual Servoing Platform  version 3.6.1 under development (2024-05-21)
#include <iostream>
#include <visp3/core/vpConfig.h>
// Check if std:c++17 or higher
#if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L))) && \
defined(HAVE_OPENCV_DNN) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(HAVE_OPENCV_HIGHGUI)) && \
#include <optional>
#include <visp3/core/vpIoTools.h>
#include <visp3/detection/vpDetectorDNNOpenCV.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/dnn_tracker/vpMegaPose.h>
#include <visp3/dnn_tracker/vpMegaPoseTracker.h>
#include <visp3/io/vpJsonArgumentParser.h>
#include <nlohmann/json.hpp>
#include <opencv2/videoio.hpp>
using json = nlohmann::json;
* Interpolate two vpColors. Linear interpolation between each components (R, G, B)
* low starting color
* high ending color
* f interpolation factor, between 0 and 1
* Returns the interpolated color
vpColor interpolate(const vpColor &low, const vpColor &high, const float f)
const float r = ((float)high.R - (float)low.R) * f;
const float g = ((float)high.G - (float)low.G) * f;
const float b = ((float)high.B - (float)low.B) * f;
return vpColor((unsigned char)r, (unsigned char)g, (unsigned char)b);
* Display the Megapose confidence score as a rectangle in the image.
* This rectangle becomes green when Megapose is "confident" about its prediction
* The confidence score measures whether Megapose can, from its pose estimation, recover the true pose in future pose refinement iterations
* \param[in] I : The image in which to display the confidence.
* \param[in] score : The confidence score of Megapose, between 0 and 1.
void displayScore(const vpImage<vpRGBa> &I, float score)
const unsigned top = static_cast<unsigned>(I.getHeight() * 0.85f);
const unsigned height = static_cast<unsigned>(I.getHeight() * 0.1f);
const unsigned left = static_cast<unsigned>(I.getWidth() * 0.05f);
const unsigned width = static_cast<unsigned>(I.getWidth() * 0.5f);
vpRect full(left, top, width, height);
vpRect scoreRect(left, top, width * score, height);
const vpColor low = vpColor::red;
const vpColor high = vpColor::green;
const vpColor c = interpolate(low, high, score);
vpDisplay::displayRectangle(I, full, c, false, 5);
vpDisplay::displayRectangle(I, scoreRect, c, true, 1);
* Add the Megapose rendering on top of the actual image I.
* Require I and overlay to be of the same size.
* Note that a fully black object will not render
void overlayRender(vpImage<vpRGBa> &I, const vpImage<vpRGBa> &overlay)
const vpRGBa black = vpRGBa(0, 0, 0);
for (unsigned int i = 0; i < I.getHeight(); ++i) {
for (unsigned int j = 0; j < I.getWidth(); ++j) {
if (overlay[i][j] != black) {
I[i][j] = overlay[i][j];
* Run the detection network on an image in order to find a specific object.
* The best matching detection is returned:
* - If a previous Megapose estimation is available, find the closest match in the image (Euclidean distance between centers)
* - Otherwise, take the detection with highest confidence
* If no detection corresponding to detectionLabel is found, then std::nullopt is returned
std::optional<vpRect> detectObjectForInitMegaposeDnn(vpDetectorDNNOpenCV &detector, const cv::Mat &I,
const std::string &detectionLabel,
std::optional<vpMegaPoseEstimate> previousEstimate)
std::vector<vpDetectorDNNOpenCV::DetectedFeatures2D> detections_vec;
detector.detect(I, detections_vec);
std::vector<vpDetectorDNNOpenCV::DetectedFeatures2D> matchingDetections;
for (const auto &detection : detections_vec) {
std::optional<std::string> classnameOpt = detection.getClassName();
if (classnameOpt) {
if (*classnameOpt == detectionLabel) {
if (matchingDetections.size() == 0) {
return std::nullopt;
else if (matchingDetections.size() == 1) {
return matchingDetections[0].getBoundingBox();
else {
// Get detection that is closest to previous object bounding box estimated by Megapose
if (previousEstimate) {
vpRect best;
double bestDist = 10000.f;
const vpImagePoint previousCenter = (*previousEstimate).boundingBox.getCenter();
for (const auto &detection : matchingDetections) {
const vpRect detectionBB = detection.getBoundingBox();
const vpImagePoint center = detectionBB.getCenter();
const double matchDist = vpImagePoint::distance(center, previousCenter);
if (matchDist < bestDist) {
bestDist = matchDist;
best = detectionBB;
return best;
else { // Get detection with highest confidence
vpRect best;
double highestConf = 0.0;
for (const auto &detection : matchingDetections) {
const double conf = detection.getConfidenceScore();
if (conf > highestConf) {
highestConf = conf;
best = detection.getBoundingBox();
return best;
return std::nullopt;
* Ask user to provide the detection themselves. They must click to start labelling, then click on the top left and bottom right corner to create the detection.
std::optional<vpRect> detectObjectForInitMegaposeClick(const vpImage<vpRGBa> &I)
const bool startLabelling = vpDisplay::getClick(I, false);
const vpImagePoint textPosition(10.0, 20.0);
if (startLabelling) {
vpImagePoint topLeft, bottomRight;
vpDisplay::displayText(I, textPosition, "Click the upper left corner of the bounding box", vpColor::red);
vpDisplay::getClick(I, topLeft, true);
vpDisplay::displayText(I, textPosition, "Click the bottom right corner of the bounding box", vpColor::red);
vpDisplay::getClick(I, bottomRight, true);
vpRect bb(topLeft, bottomRight);
return bb;
else {
vpDisplay::displayText(I, textPosition, "Click when the object is visible and static to start reinitializing megapose.", vpColor::red);
return std::nullopt;
enum DetectionMethod
{UNKNOWN, nullptr}, // Default value if the json string is not in "current", "desired" or "mean"
{CLICK, "click"},
{DNN, "dnn"} }
int main(int argc, const char *argv[])
unsigned width = 640, height = 480;
std::string videoDevice = "0";
std::string megaposeAddress = "";
unsigned megaposePort = 5555;
int refinerIterations = 1, coarseNumSamples = 576;
double reinitThreshold = 0.2;
DetectionMethod detectionMethod = DetectionMethod::UNKNOWN;
std::string detectorModelPath = "path/to/model.onnx", detectorConfig = "none";
std::string detectorFramework = "onnx", detectorTypeString = "yolov7";
std::string objectName = "cube";
std::vector<std::string> labels = { "cube" };
float detectorMeanR = 0.f, detectorMeanG = 0.f, detectorMeanB = 0.f;
float detectorConfidenceThreshold = 0.65f, detectorNmsThreshold = 0.5f, detectorFilterThreshold = -0.25f;
float detectorScaleFactor = 0.0039f;
bool detectorSwapRB = false;
vpJsonArgumentParser parser("Single object tracking with Megapose", "--config", "/");
parser.addArgument("width", width, true, "The image width")
.addArgument("height", height, true, "The image height")
.addArgument("camera", cam, true, "The camera intrinsic parameters. Should correspond to a perspective projection model without distortion.")
.addArgument("video-device", videoDevice, true, "Video device")
.addArgument("object", objectName, true, "Name of the object to track with megapose.")
.addArgument("detectionMethod", detectionMethod, true, "How to perform detection of the object to get the bounding box:"
" \"click\" for user labelling, \"dnn\" for dnn detection.")
.addArgument("reinitThreshold", reinitThreshold, false, "If the Megapose score falls below this threshold, then a reinitialization is be required."
" Should be between 0 and 1")
.addArgument("megapose/address", megaposeAddress, true, "IP address of the Megapose server.")
.addArgument("megapose/port", megaposePort, true, "Port on which the Megapose server listens for connections.")
.addArgument("megapose/refinerIterations", refinerIterations, false, "Number of Megapose refiner model iterations."
"A higher count may lead to better accuracy, at the cost of more processing time")
.addArgument("megapose/initialisationNumSamples", coarseNumSamples, false, "Number of Megapose renderings used for the initial pose estimation.")
.addArgument("detector/model-path", detectorModelPath, true, "Path to the model")
.addArgument("detector/config", detectorConfig, true, "Path to the model configuration. Set to none if config is not required.")
.addArgument("detector/framework", detectorFramework, true, "Detector framework")
.addArgument("detector/type", detectorTypeString, true, "Detector type")
.addArgument("detector/labels", labels, true, "Detection class labels")
.addArgument("detector/mean/red", detectorMeanR, false, "Detector mean red component. Used to normalize image")
.addArgument("detector/mean/green", detectorMeanG, false, "Detector mean green component. Used to normalize image")
.addArgument("detector/mean/blue", detectorMeanB, false, "Detector mean red component. Used to normalize image")
.addArgument("detector/confidenceThreshold", detectorConfidenceThreshold, false, "Detector confidence threshold. "
"When a detection with a confidence below this threshold, it is ignored")
.addArgument("detector/nmsThreshold", detectorNmsThreshold, false, "Detector non maximal suppression threshold.")
.addArgument("detector/filterThreshold", detectorFilterThreshold, false)
.addArgument("detector/scaleFactor", detectorScaleFactor, false, "Pixel intensity rescaling factor. If set to 1/255, then pixel values are between 0 and 1.")
.addArgument("detector/swapRedAndBlue", detectorSwapRB, false, "Whether to swap red and blue channels before feeding the image to the detector.");
parser.parse(argc, argv);
throw vpException(vpException::badValue, "The camera projection model should be without distortion, as other models are ignored by Megapose");
if (detectionMethod == DetectionMethod::UNKNOWN) {
throw vpException(vpException::badValue, "The specified detection method is incorrect: it should be either \"click\" or \"dnn\"");
cv::VideoCapture capture;
bool isLiveCapture;
bool hasCaptureOpeningSucceeded;
double videoFrametime = 0; // Only for prerecorded videos
if (vpMath::isNumber(videoDevice)) {
hasCaptureOpeningSucceeded =;
isLiveCapture = true;
else {
hasCaptureOpeningSucceeded =;
isLiveCapture = false;
double fps = capture.get(cv::CAP_PROP_FPS);
videoFrametime = (1.0 / fps) * 1000.0;
if (!hasCaptureOpeningSucceeded) {
std::cout << "Capture from camera: " << videoDevice << " didn't work" << std::endl;
#if defined(VISP_HAVE_X11)
#elif defined(VISP_HAVE_GDI)
#elif defined(HAVE_OPENCV_HIGHGUI)
#if (VISP_HAVE_OPENCV_VERSION >= 0x030403) && defined(HAVE_OPENCV_DNN) && \
((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
vpDetectorDNNOpenCV::DNNResultsParsingType detectorType =
vpDetectorDNNOpenCV::NetConfig netConfig(detectorConfidenceThreshold, detectorNmsThreshold, labels,
cv::Size(width, height), detectorFilterThreshold);
vpDetectorDNNOpenCV dnn(netConfig, detectorType);
if (detectionMethod == DetectionMethod::DNN) {
dnn.readNet(detectorModelPath, detectorConfig, detectorFramework);
dnn.setMean(detectorMeanR, detectorMeanG, detectorMeanB);
std::shared_ptr<vpMegaPose> megapose;
try {
megapose = std::make_shared<vpMegaPose>(megaposeAddress, megaposePort, cam, height, width);
catch (...) {
throw vpException(vpException::ioError, "Could not connect to Megapose server at " + megaposeAddress + " on port " + std::to_string(megaposePort));
vpMegaPoseTracker megaposeTracker(megapose, objectName, refinerIterations);
const std::vector<std::string> allObjects = megapose->getObjectNames();
if (std::find(allObjects.begin(), allObjects.end(), objectName) == allObjects.end()) {
throw vpException(vpException::badValue, "Object " + objectName + " is not known by the Megapose server!");
std::future<vpMegaPoseEstimate> trackerFuture;
cv::Mat frame;
vpMegaPoseEstimate megaposeEstimate; // last Megapose estimation
vpRect lastDetection; // Last detection (initialization)
bool callMegapose = true; // Whether we should call Megapose this iteration
bool initialized = false; // Whether tracking should be initialized or reinitialized
bool tracking = false;
bool overlayModel = true;
vpImage<vpRGBa> overlayImage(height, width);
std::string overlayMode = "full";
std::vector<double> megaposeTimes;
std::vector<double> frameTimes;
double megaposeStartTime = 0.0;
while (true) {
const double frameStart = vpTime::measureTimeMs();
capture >> frame;
if (frame.empty())
if (I.getSize() == 0) {
vpDisplay::setTitle(I, "Megapose object pose estimation");
else {
// Check whether Megapose is still running
if (!callMegapose && trackerFuture.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) {
megaposeEstimate = trackerFuture.get();
if (tracking) {
megaposeTimes.push_back(vpTime::measureTimeMs() - megaposeStartTime);
callMegapose = true;
tracking = true;
if (overlayModel) {
overlayImage = megapose->viewObjects({ objectName }, { megaposeEstimate.cTo }, overlayMode);
if (megaposeEstimate.score < reinitThreshold) { // If confidence is low, require a reinitialisation with 2D detection
initialized = false;
if (callMegapose) {
if (!initialized) {
tracking = false;
std::optional<vpRect> detection = std::nullopt;
#if (VISP_HAVE_OPENCV_VERSION >= 0x030403) && defined(HAVE_OPENCV_DNN) && \
((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
if (detectionMethod == DetectionMethod::DNN) {
detection = detectObjectForInitMegaposeDnn(
dnn, frame, objectName, initialized ? std::optional(megaposeEstimate) : std::nullopt);
if (detectionMethod == DetectionMethod::CLICK) {
detection = detectObjectForInitMegaposeClick(I);
if (detection) {
initialized = true;
lastDetection = *detection;
trackerFuture = megaposeTracker.init(I, lastDetection);
callMegapose = false;
else {
trackerFuture = megaposeTracker.track(I);
callMegapose = false;
megaposeStartTime = vpTime::measureTimeMs();
std::string keyboardEvent;
const bool keyPressed = vpDisplay::getKeyboardEvent(I, keyboardEvent, false);
if (keyPressed) {
if (keyboardEvent == "t") {
overlayModel = !overlayModel;
else if (keyboardEvent == "w") {
overlayMode = overlayMode == "full" ? "wireframe" : "full";
if (tracking) {
if (overlayModel) {
overlayRender(I, overlayImage);
vpDisplay::displayText(I, 20, 20, "Right click to quit", vpColor::red);
vpDisplay::displayText(I, 30, 20, "Press T: Toggle overlay", vpColor::red);
vpDisplay::displayText(I, 40, 20, "Press W: Toggle wireframe", vpColor::red);
vpDisplay::displayFrame(I, megaposeEstimate.cTo, cam, 0.05, vpColor::none, 3);
//vpDisplay::displayRectangle(I, lastDetection, vpColor::red);
displayScore(I, megaposeEstimate.score);
if (vpDisplay::getClick(I, button, false)) {
if (button == vpMouseButton::button3) {
break; // Right click to stop
const double frameEnd = vpTime::measureTimeMs();
if (!isLiveCapture) {
vpTime::wait(std::max<double>(0.0, videoFrametime - (frameEnd - frameStart)));
frameTimes.push_back(vpTime::measureTimeMs() - frameStart);
std::cout << "Average frame time: " << vpMath::getMean(frameTimes) << std::endl;
std::cout << "Average time between Megapose calls: " << vpMath::getMean(megaposeTimes) << std::endl;
int main()
std::cout << "Compile ViSP with the DNN tracker module, the JSON 3rd party library and the OpenCV detection module" << std::endl;
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
vpCameraParametersProjType get_projModel() const
Class to define RGB colors available for display functionalities.
Definition: vpColor.h:152
static const vpColor red
Definition: vpColor.h:211
static const vpColor none
Definition: vpColor.h:223
static const vpColor green
Definition: vpColor.h:214
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:128
The vpDisplayOpenCV allows to display image using the OpenCV library. Thus to enable this class OpenC...
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:128
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="") vp_override
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static bool getKeyboardEvent(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 displayCross(const vpImage< unsigned char > &I, const vpImagePoint &ip, unsigned int size, const vpColor &color, unsigned int thickness=1)
static void setTitle(const vpImage< unsigned char > &I, const std::string &windowtitle)
static void flush(const vpImage< unsigned char > &I)
static void displayRectangle(const vpImage< unsigned char > &I, const vpImagePoint &topLeft, unsigned int width, unsigned int height, const vpColor &color, bool fill=false, unsigned int thickness=1)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ ioError
I/O error.
Definition: vpException.h:79
@ badValue
Used to indicate that a value is not in the allowed range.
Definition: vpException.h:85
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
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 distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
unsigned int getWidth() const
Definition: vpImage.h:245
unsigned int getSize() const
Definition: vpImage.h:224
unsigned int getHeight() const
Definition: vpImage.h:184
Command line argument parsing with support for JSON files. If a JSON file is supplied,...
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:303
static bool isNumber(const std::string &str)
Definition: vpMath.cpp:215
vpHomogeneousMatrix cTo
Definition: vpMegaPose.h:69
A simplified interface to track a single object with MegaPose. This tracker works asynchronously: A c...
Definition: vpRGBa.h:61
unsigned char B
Blue component.
Definition: vpRGBa.h:139
unsigned char R
Red component.
Definition: vpRGBa.h:137
unsigned char G
Green component.
Definition: vpRGBa.h:138
Defines a rectangle in the plane.
Definition: vpRect.h:76
void getCenter(double &x, double &y) const
Definition: vpRect.h:133
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()