2#include <visp3/core/vpConfig.h>
3#ifdef VISP_HAVE_MODULE_SENSOR
4#include <visp3/sensor/vpRealSense2.h>
6#include <visp3/gui/vpDisplayGDI.h>
7#include <visp3/gui/vpDisplayOpenCV.h>
8#include <visp3/gui/vpDisplayX.h>
10#include "pose_helper.h"
12int main(
int argc,
char **argv)
14#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
15 defined(VISP_HAVE_REALSENSE2) && (RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
17 double opt_square_width = 0.12;
18 int opt_camera_index = 1;
20 for (
int i = 0; i < argc; i++) {
21 if (std::string(argv[i]) ==
"--camera_index" && i + 1 < argc) {
22 opt_camera_index = atoi(argv[i + 1]);
23 }
else if (std::string(argv[i]) ==
"--square_width" && i + 1 < argc) {
24 opt_square_width = atoi(argv[i + 1]);
25 }
else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
26 std::cout <<
"\nUsage: " << argv[0] <<
" [--camera_index <1.Left | 2.Right> (default: 1)]"
27 <<
" [--square_width <square width in meter (default: 0.12)] [--help] [-h]\n"
28 <<
"\nExample using right camera and square size 0.1:\n"
29 <<
" " << argv[0] <<
"--camera_index 2 --square_width 0.1\n"
38 std::cout <<
"Use Realsense 2 grabber" << std::endl;
41 config.enable_stream(RS2_STREAM_FISHEYE, 1);
42 config.enable_stream(RS2_STREAM_FISHEYE, 2);
45 if (opt_camera_index == 1)
50 std::cout <<
"Read camera parameters from Realsense device" << std::endl;
56 std::cout <<
"Square width : " << opt_square_width << std::endl;
57 std::cout << cam << std::endl;
62 std::vector<vpDot2> dot(4);
63 std::vector<vpPoint> point;
64 std::vector<vpImagePoint> ip;
65 double L = opt_square_width / 2.;
66 point.push_back(
vpPoint(-L, -L, 0));
67 point.push_back(
vpPoint(L, -L, 0));
68 point.push_back(
vpPoint(L, L, 0));
69 point.push_back(
vpPoint(-L, L, 0));
71#if defined(VISP_HAVE_X11)
73#elif defined(VISP_HAVE_GDI)
75#elif defined(HAVE_OPENCV_HIGHGUI)
80 bool apply_cv =
false;
86 if (opt_camera_index == 1)
94 ip = track(I, dot, init_cv);
95 computePose(point, ip, cam, init_cv, cMo);
102 std::stringstream ss;
103 ss <<
"Translation: " << std::setprecision(5) << pose[0] <<
" " << pose[1] <<
" " << pose[2] <<
" [m]";
106 ss <<
"Rotation tu: " << std::setprecision(4) <<
vpMath::deg(pose[3]) <<
" " <<
vpMath::deg(pose[4]) <<
" "
111 std::cout <<
"Computer vision failure." << std::endl;
131 std::stringstream ss;
138 std::cout <<
"Catch an exception: " << e.
getMessage() << std::endl;
140#elif !defined(VISP_HAVE_REALSENSE2)
143 std::cout <<
"You do not realsense2 SDK functionality enabled..." << std::endl;
144 std::cout <<
"Tip:" << std::endl;
145 std::cout <<
"- Install librealsense2, configure again ViSP using cmake and build again this example" << std::endl;
147#elif (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
150 std::cout <<
"You do not build ViSP with c++11 or higher compiler flag" << std::endl;
151 std::cout <<
"Tip:" << std::endl;
152 std::cout <<
"- Configure ViSP again using cmake -DUSE_CXX_STANDARD=11, and build again this example" << std::endl;
153#elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
156 std::cout <<
"Install a 3rd party dedicated to image display (X11, GDI, OpenCV), configure and build ViSP again to "
159#elif !(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
162 std::cout <<
"Install librealsense version > 2.31.0" << std::endl;
Generic class defining intrinsic camera parameters.
@ ProjWithKannalaBrandtDistortion
Projection with Kannala-Brandt 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...
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 char * getMessage() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
Definition of the vpImage class member functions.
unsigned int getWidth() const
static double deg(double rad)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Implementation of a pose vector and operations on poses.
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
bool open(const rs2::config &cfg=rs2::config())
VISP_EXPORT double measureTimeMs()