4#include <visp3/core/vpConfig.h>
5#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && defined(VISP_HAVE_NLOHMANN_JSON) && defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_VIDEOIO) && defined(HAVE_OPENCV_DNN) && \
6 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(HAVE_OPENCV_HIGHGUI))
10#include <visp3/core/vpIoTools.h>
11#include <visp3/detection/vpDetectorDNNOpenCV.h>
12#include <visp3/gui/vpDisplayGDI.h>
13#include <visp3/gui/vpDisplayOpenCV.h>
14#include <visp3/gui/vpDisplayX.h>
15#include <visp3/dnn_tracker/vpMegaPose.h>
16#include <visp3/dnn_tracker/vpMegaPoseTracker.h>
17#include <visp3/io/vpJsonArgumentParser.h>
19#include <nlohmann/json.hpp>
21#include <opencv2/videoio.hpp>
24using json = nlohmann::json;
36 const float r = ((float)high.
R - (
float)low.
R) * f;
37 const float g = ((float)high.
G - (
float)low.
G) * f;
38 const float b = ((float)high.
B - (
float)low.
B) * f;
39 return vpColor((
unsigned char)r, (
unsigned char)g, (
unsigned char)b);
52 const unsigned top =
static_cast<unsigned>(I.
getHeight() * 0.85f);
53 const unsigned height =
static_cast<unsigned>(I.
getHeight() * 0.1f);
54 const unsigned left =
static_cast<unsigned>(I.
getWidth() * 0.05f);
55 const unsigned width =
static_cast<unsigned>(I.
getWidth() * 0.5f);
56 vpRect full(left, top, width, height);
57 vpRect scoreRect(left, top, width * score, height);
60 const vpColor c = interpolate(low, high, score);
74 for (
unsigned int i = 0; i < I.
getHeight(); ++i) {
75 for (
unsigned int j = 0; j < I.
getWidth(); ++j) {
76 if (overlay[i][j] != black) {
77 I[i][j] = overlay[i][j];
91std::optional<vpRect> detectObjectForInitMegaposeDnn(
vpDetectorDNNOpenCV &detector,
const cv::Mat &I,
92 const std::string &detectionLabel,
93 std::optional<vpMegaPoseEstimate> previousEstimate)
95 std::vector<vpDetectorDNNOpenCV::DetectedFeatures2D> detections_vec;
96 detector.
detect(I, detections_vec);
97 std::vector<vpDetectorDNNOpenCV::DetectedFeatures2D> matchingDetections;
98 for (
const auto &detection : detections_vec) {
99 std::optional<std::string> classnameOpt = detection.getClassName();
101 if (*classnameOpt == detectionLabel) {
102 matchingDetections.push_back(detection);
106 if (matchingDetections.size() == 0) {
109 else if (matchingDetections.size() == 1) {
110 return matchingDetections[0].getBoundingBox();
114 if (previousEstimate) {
116 double bestDist = 10000.f;
117 const vpImagePoint previousCenter = (*previousEstimate).boundingBox.getCenter();
118 for (
const auto &detection : matchingDetections) {
119 const vpRect detectionBB = detection.getBoundingBox();
122 if (matchDist < bestDist) {
123 bestDist = matchDist;
132 double highestConf = 0.0;
133 for (
const auto &detection : matchingDetections) {
134 const double conf = detection.getConfidenceScore();
135 if (conf > highestConf) {
137 best = detection.getBoundingBox();
149std::optional<vpRect> detectObjectForInitMegaposeClick(
const vpImage<vpRGBa> &I)
155 if (startLabelling) {
165 vpRect bb(topLeft, bottomRight);
184NLOHMANN_JSON_SERIALIZE_ENUM(DetectionMethod, {
191int main(
int argc,
const char *argv [])
193 unsigned width = 640, height = 480;
195 std::string videoDevice =
"0";
196 std::string megaposeAddress =
"127.0.0.1";
197 unsigned megaposePort = 5555;
198 int refinerIterations = 1, coarseNumSamples = 576;
199 double reinitThreshold = 0.2;
201 DetectionMethod detectionMethod = DetectionMethod::UNKNOWN;
203 std::string detectorModelPath =
"path/to/model.onnx", detectorConfig =
"none";
204 std::string detectorFramework =
"onnx", detectorTypeString =
"yolov7";
205 std::string objectName =
"cube";
206 std::vector<std::string> labels = {
"cube" };
207 float detectorMeanR = 0.f, detectorMeanG = 0.f, detectorMeanB = 0.f;
208 float detectorConfidenceThreshold = 0.65f, detectorNmsThreshold = 0.5f, detectorFilterThreshold = -0.25f;
209 float detectorScaleFactor = 0.0039f;
210 bool detectorSwapRB =
false;
213 parser.addArgument(
"width", width,
true,
"The image width")
214 .addArgument(
"height", height,
true,
"The image height")
215 .addArgument(
"camera", cam,
true,
"The camera intrinsic parameters. Should correspond to a perspective projection model without distortion.")
216 .addArgument(
"video-device", videoDevice,
true,
"Video device")
217 .addArgument(
"object", objectName,
true,
"Name of the object to track with megapose.")
218 .addArgument(
"detectionMethod", detectionMethod,
true,
"How to perform detection of the object to get the bounding box:"
219 " \"click\" for user labelling, \"dnn\" for dnn detection.")
220 .addArgument(
"reinitThreshold", reinitThreshold,
false,
"If the Megapose score falls below this threshold, then a reinitialization is be required."
221 " Should be between 0 and 1")
222 .addArgument(
"megapose/address", megaposeAddress,
true,
"IP address of the Megapose server.")
223 .addArgument(
"megapose/port", megaposePort,
true,
"Port on which the Megapose server listens for connections.")
224 .addArgument(
"megapose/refinerIterations", refinerIterations,
false,
"Number of Megapose refiner model iterations."
225 "A higher count may lead to better accuracy, at the cost of more processing time")
226 .addArgument(
"megapose/initialisationNumSamples", coarseNumSamples,
false,
"Number of Megapose renderings used for the initial pose estimation.")
228 .addArgument(
"detector/model-path", detectorModelPath,
true,
"Path to the model")
229 .addArgument(
"detector/config", detectorConfig,
true,
"Path to the model configuration. Set to none if config is not required.")
230 .addArgument(
"detector/framework", detectorFramework,
true,
"Detector framework")
231 .addArgument(
"detector/type", detectorTypeString,
true,
"Detector type")
232 .addArgument(
"detector/labels", labels,
true,
"Detection class labels")
233 .addArgument(
"detector/mean/red", detectorMeanR,
false,
"Detector mean red component. Used to normalize image")
234 .addArgument(
"detector/mean/green", detectorMeanG,
false,
"Detector mean green component. Used to normalize image")
235 .addArgument(
"detector/mean/blue", detectorMeanB,
false,
"Detector mean red component. Used to normalize image")
236 .addArgument(
"detector/confidenceThreshold", detectorConfidenceThreshold,
false,
"Detector confidence threshold. "
237 "When a detection with a confidence below this threshold, it is ignored")
238 .addArgument(
"detector/nmsThreshold", detectorNmsThreshold,
false,
"Detector non maximal suppression threshold.")
239 .addArgument(
"detector/filterThreshold", detectorFilterThreshold,
false)
240 .addArgument(
"detector/scaleFactor", detectorScaleFactor,
false,
"Pixel intensity rescaling factor. If set to 1/255, then pixel values are between 0 and 1.")
241 .addArgument(
"detector/swapRedAndBlue", detectorSwapRB,
false,
"Whether to swap red and blue channels before feeding the image to the detector.");
243 parser.parse(argc, argv);
250 if (detectionMethod == DetectionMethod::UNKNOWN) {
254 cv::VideoCapture capture;
256 bool hasCaptureOpeningSucceeded;
257 double videoFrametime = 0;
259 hasCaptureOpeningSucceeded = capture.open(std::atoi(videoDevice.c_str()));
260 isLiveCapture =
true;
263 hasCaptureOpeningSucceeded = capture.open(videoDevice);
264 isLiveCapture =
false;
265 double fps = capture.get(cv::CAP_PROP_FPS);
266 videoFrametime = (1.0 / fps) * 1000.0;
268 if (!hasCaptureOpeningSucceeded) {
269 std::cout <<
"Capture from camera: " << videoDevice <<
" didn't work" << std::endl;
274#if defined(VISP_HAVE_X11)
276#elif defined(VISP_HAVE_GDI)
278#elif defined(HAVE_OPENCV_HIGHGUI)
282#if (VISP_HAVE_OPENCV_VERSION >= 0x030403) && defined(HAVE_OPENCV_DNN) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
286 cv::Size(width, height), detectorFilterThreshold);
288 if (detectionMethod == DetectionMethod::DNN) {
289 dnn.readNet(detectorModelPath, detectorConfig, detectorFramework);
290 dnn.setMean(detectorMeanR, detectorMeanG, detectorMeanB);
291 dnn.setScaleFactor(detectorScaleFactor);
292 dnn.setSwapRB(detectorSwapRB);
296 std::shared_ptr<vpMegaPose> megapose;
298 megapose = std::make_shared<vpMegaPose>(megaposeAddress, megaposePort, cam, height, width);
305 megapose->setCoarseNumSamples(coarseNumSamples);
306 const std::vector<std::string> allObjects = megapose->getObjectNames();
307 if (std::find(allObjects.begin(), allObjects.end(), objectName) == allObjects.end()) {
310 std::future<vpMegaPoseEstimate> trackerFuture;
316 bool callMegapose =
true;
317 bool initialized =
false;
318 bool tracking =
false;
320 bool overlayModel =
true;
322 std::string overlayMode =
"full";
324 std::vector<double> megaposeTimes;
325 std::vector<double> frameTimes;
327 double megaposeStartTime = 0.0;
348 if (!callMegapose && trackerFuture.wait_for(std::chrono::milliseconds(0)) == std::future_status::ready) {
349 megaposeEstimate = trackerFuture.get();
357 overlayImage = megapose->viewObjects({ objectName }, { megaposeEstimate.
cTo }, overlayMode);
360 if (megaposeEstimate.
score < reinitThreshold) {
369 std::optional<vpRect> detection = std::nullopt;
370#if (VISP_HAVE_OPENCV_VERSION >= 0x030403) && defined(HAVE_OPENCV_DNN) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)
371 if (detectionMethod == DetectionMethod::DNN) {
372 detection = detectObjectForInitMegaposeDnn(
373 dnn, frame, objectName, initialized ? std::optional(megaposeEstimate) : std::nullopt);
376 if (detectionMethod == DetectionMethod::CLICK) {
377 detection = detectObjectForInitMegaposeClick(I);
382 lastDetection = *detection;
383 trackerFuture = megaposeTracker.init(I, lastDetection);
384 callMegapose =
false;
389 trackerFuture = megaposeTracker.track(I);
390 callMegapose =
false;
397 std::string keyboardEvent;
400 if (keyboardEvent ==
"t") {
401 overlayModel = !overlayModel;
403 else if (keyboardEvent ==
"w") {
404 overlayMode = overlayMode ==
"full" ?
"wireframe" :
"full";
410 overlayRender(I, overlayImage);
418 displayScore(I, megaposeEstimate.
score);
431 if (!isLiveCapture) {
432 vpTime::wait(std::max(0.0, videoFrametime - (frameEnd - frameStart)));
436 std::cout <<
"Average frame time: " <<
vpMath::getMean(frameTimes) << std::endl;
437 std::cout <<
"Average time between Megapose calls: " <<
vpMath::getMean(megaposeTimes) << std::endl;
443 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.
static const vpColor none
static const vpColor green
Structure containing some information required for the configuration of a vpDetectorDNNOpenCV object.
DNNResultsParsingType
Enumeration listing the types of DNN for which the vpDetectorDNNOpenCV furnishes the methods permitti...
static DNNResultsParsingType dnnResultsParsingTypeFromString(const std::string &name)
virtual bool detect(const vpImage< unsigned char > &I, std::vector< DetectedFeatures2D > &output)
Object detection using OpenCV DNN module.
Display for windows using GDI (available on any windows 32 platform).
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...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
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.
@ badValue
Used to indicate that a value is not in the allowed range.
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 ...
static double distance(const vpImagePoint &iP1, const vpImagePoint &iP2)
Definition of the vpImage class member functions.
unsigned int getWidth() const
unsigned int getSize() const
unsigned int getHeight() const
Command line argument parsing with support for JSON files. If a JSON file is supplied,...
static double getMean(const std::vector< double > &v)
static bool isNumber(const std::string &str)
A simplified interface to track a single object with MegaPose. This tracker works asynchronously: A c...
unsigned char B
Blue component.
unsigned char R
Red component.
unsigned char G
Green component.
Defines a rectangle in the plane.
void getCenter(double &x, double &y) const
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT double measureTimeMs()