4 #include <visp3/core/vpConfig.h>
6 #if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_OPENCV) && defined(VISP_HAVE_NLOHMANN_JSON)
7 #include <visp3/core/vpDisplay.h>
8 #include <visp3/core/vpIoTools.h>
9 #include <visp3/gui/vpDisplayGDI.h>
10 #include <visp3/gui/vpDisplayOpenCV.h>
11 #include <visp3/gui/vpDisplayX.h>
12 #include <visp3/mbt/vpMbGenericTracker.h>
13 #include <visp3/sensor/vpRealSense2.h>
15 #include <nlohmann/json.hpp>
16 using json = nlohmann::json;
19 int main(
int argc,
char *argv [])
21 std::string config_file =
"";
22 std::string model =
"";
23 std::string init_file =
"";
25 double proj_error_threshold = 25;
27 for (
int i = 1; i < argc; i++) {
28 if (std::string(argv[i]) ==
"--config" && i + 1 < argc) {
29 config_file = std::string(argv[i + 1]);
31 else if (std::string(argv[i]) ==
"--model" && i + 1 < argc) {
32 model = std::string(argv[i + 1]);
34 else if (std::string(argv[i]) ==
"--init_file" && i + 1 < argc) {
35 init_file = std::string(argv[i + 1]);
37 else if (std::string(argv[i]) ==
"--proj_error_threshold" && i + 1 < argc) {
38 proj_error_threshold = std::atof(argv[i + 1]);
40 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
41 std::cout <<
"Usage: \n"
43 <<
"--config <settings.json>"
44 <<
" --model <object.cao>"
45 " --init_file <object.init>"
46 " [--proj_error_threshold <threshold between 0 and 90> (default: "
47 << proj_error_threshold
51 std::cout <<
"\n** How to track a 4.2 cm width cube with manual initialization:\n"
52 << argv[0] <<
"--config model/cube/rgbd-tracker.json --model model/cube/cube.cao" << std::endl;
57 std::cout <<
"Config files: " << std::endl;
58 std::cout <<
" JSON config: "
59 <<
"\"" << config_file <<
"\"" << std::endl;
60 std::cout <<
" Model: "
61 <<
"\"" << model <<
"\"" << std::endl;
62 std::cout <<
" Init file: "
63 <<
"\"" << init_file <<
"\"" << std::endl;
65 if (config_file.empty()) {
66 std::cout <<
"No JSON configuration was provided!" << std::endl;
71 int width = 640, height = 480;
74 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, fps);
75 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
78 realsense.
open(config);
81 std::cout <<
"Catch an exception: " << e.
what() << std::endl;
82 std::cout <<
"Check if the Realsense camera is connected..." << std::endl;
91 std::cout <<
"Sensor internal camera parameters for color camera: " << cam_color << std::endl;
92 std::cout <<
"Sensor internal camera parameters for depth camera: " << cam_depth << std::endl;
98 std::vector<vpColVector> pointcloud;
102 std::map<std::string, vpHomogeneousMatrix> mapOfCameraTransformations;
103 std::map<std::string, const vpImage<unsigned char> *> mapOfImages;
104 std::map<std::string, std::string> mapOfInitFiles;
105 std::map<std::string, const std::vector<vpColVector> *> mapOfPointclouds;
106 std::map<std::string, unsigned int> mapOfWidths, mapOfHeights;
107 std::map<std::string, vpCameraParameters> mapOfCameraIntrinsics;
113 if (model.empty() && init_file.empty()) {
114 std::ifstream config(config_file);
115 const json j = json::parse(config);
117 if (j.contains(
"model")) {
121 std::cerr <<
"No model was provided in either JSON file or arguments" << std::endl;
125 if (init_file.empty()) {
127 init_file = (parentname.empty() ?
"" : (parentname +
"/")) +
vpIoTools::getNameWE(model) +
".init";
131 std::string color_key =
"", depth_key =
"";
133 std::cout <<
"tracker key == " << tracker_type.first << std::endl;
136 color_key = tracker_type.first;
137 mapOfImages[color_key] = &I_gray;
138 mapOfInitFiles[color_key] = init_file;
139 mapOfWidths[color_key] = width;
140 mapOfHeights[color_key] = height;
141 mapOfCameraIntrinsics[color_key] = cam_color;
145 depth_key = tracker_type.first;
146 mapOfImages[depth_key] = &I_depth;
147 mapOfWidths[depth_key] = width;
148 mapOfHeights[depth_key] = height;
149 mapOfCameraIntrinsics[depth_key] = cam_depth;
150 mapOfCameraTransformations[depth_key] = depth_M_color;
151 mapOfPointclouds[depth_key] = &pointcloud;
154 const bool use_depth = !depth_key.empty();
155 const bool use_color = !color_key.empty();
164 std::cout <<
"Updating configuration with parameters provided by RealSense SDK..." << std::endl;
166 if (use_color && use_depth) {
170 unsigned int _posx = 100, _posy = 50;
174 #elif defined(VISP_HAVE_GDI)
176 #elif defined(HAVE_OPENCV_HIGHGUI)
180 d1.
init(I_gray, _posx, _posy,
"Color stream");
182 d2.
init(I_depth, _posx + I_gray.getWidth() + 10, _posy,
"Depth stream");
185 realsense.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap,
nullptr,
nullptr);
212 tracker.
initClick(mapOfImages, mapOfInitFiles,
true);
215 std::vector<double> times_vec;
223 bool tracking_failed =
false;
226 realsense.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap, &pointcloud,
nullptr,
nullptr);
237 if (use_color && use_depth) {
238 mapOfImages[color_key] = &I_gray;
239 mapOfPointclouds[depth_key] = &pointcloud;
240 mapOfWidths[depth_key] = width;
241 mapOfHeights[depth_key] = height;
243 else if (use_color) {
244 mapOfImages[color_key] = &I_gray;
246 else if (use_depth) {
247 mapOfPointclouds[depth_key] = &pointcloud;
252 if (use_color && use_depth) {
253 tracker.
track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
255 else if (use_color) {
256 tracker.
track(I_gray);
258 else if (use_depth) {
259 tracker.
track(mapOfImages, mapOfPointclouds, mapOfWidths, mapOfHeights);
264 tracking_failed =
true;
271 double proj_error = 0;
280 if (proj_error > proj_error_threshold) {
281 std::cout <<
"Tracker needs to restart (projection error detected: " << proj_error <<
")" << std::endl;
285 if (!tracking_failed) {
289 if (use_color && use_depth) {
290 tracker.
display(I_gray, I_depth, cMo, depth_M_color * cMo, cam_color, cam_depth,
vpColor::red, 3);
294 else if (use_color) {
298 else if (use_depth) {
304 std::stringstream ss;
309 std::stringstream ss;
316 std::stringstream ss;
318 times_vec.push_back(loop_t);
319 ss <<
"Loop time: " << loop_t <<
" ms";
347 std::cout <<
"Caught an exception: " << e.
what() << std::endl;
350 if (!times_vec.empty()) {
358 #elif defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_OPENCV)
361 std::cout <<
"Install the JSON 3rd party library (Nlohmann JSON), reconfigure VISP and build again" << std::endl;
364 #elif defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_NLOHMANN_JSON)
367 std::cout <<
"Install OpenCV, reconfigure VISP and build again" << std::endl;
373 std::cout <<
"Install librealsense2 3rd party, configure and build ViSP again to use this example" << std::endl;
unsigned int size() const
Return the number of elements of the 2D array.
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static const vpColor none
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="") vp_override
static bool getClick(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 flush(const vpImage< unsigned char > &I)
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.
const std::string & getStringMessage() const
const char * what() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
static double getMedian(const std::vector< double > &v)
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
static double getMean(const std::vector< double > &v)
Real-time 6D object pose tracking using its CAD model.
virtual int getTrackerType() const
virtual void loadConfigFile(const std::string &configFile, bool verbose=true) vp_override
virtual unsigned int getNbFeaturesEdge() const
virtual unsigned int getNbPolygon() const vp_override
virtual void getPose(vpHomogeneousMatrix &cMo) const vp_override
virtual void display(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &cMo, const vpCameraParameters &cam, const vpColor &col, unsigned int thickness=1, bool displayFullModel=false) vp_override
virtual std::map< std::string, int > getCameraTrackerTypes() const
virtual unsigned int getNbFeaturesKlt() const
virtual unsigned int getNbFeaturesDepthDense() const
virtual void loadModel(const std::string &modelFile, bool verbose=false, const vpHomogeneousMatrix &T=vpHomogeneousMatrix()) vp_override
virtual unsigned int getNbFeaturesDepthNormal() const
virtual void setCameraParameters(const vpCameraParameters &camera) vp_override
virtual void initClick(const vpImage< unsigned char > &I1, const vpImage< unsigned char > &I2, const std::string &initFile1, const std::string &initFile2, bool displayHelp=false, const vpHomogeneousMatrix &T1=vpHomogeneousMatrix(), const vpHomogeneousMatrix &T2=vpHomogeneousMatrix())
virtual double computeCurrentProjectionError(const vpImage< unsigned char > &I, const vpHomogeneousMatrix &_cMo, const vpCameraParameters &_cam) vp_override
virtual void setDisplayFeatures(bool displayF) vp_override
virtual void setProjectionErrorComputation(const bool &flag) vp_override
virtual vpColVector getError() const vp_override
virtual void setCameraTransformationMatrix(const std::string &cameraName, const vpHomogeneousMatrix &cameraTransformationMatrix)
virtual void track(const vpImage< unsigned char > &I) vp_override
virtual double getProjectionError() const
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
bool open(const rs2::config &cfg=rs2::config())
vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to, int from_index=-1) const
VISP_EXPORT double measureTimeMs()