#include <visp3/core/vpConfig.h>
#ifdef VISP_HAVE_MODULE_SENSOR
#include <visp3/sensor/vpRealSense2.h>
#endif
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include "pose_helper.h"
int main(int argc, char **argv)
{
#if (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV)) && \
defined(VISP_HAVE_REALSENSE2) && \
(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
try {
double opt_square_width = 0.12;
int opt_camera_index = 1;
for (int i = 0; i < argc; i++) {
if (std::string(argv[i]) == "--camera_index" && i + 1 < argc) {
opt_camera_index = atoi(argv[i + 1]);
} else if(std::string(argv[i]) == "--square_width" && i + 1 < argc) {
opt_square_width = atoi(argv[i + 1]);
} else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
std::cout << "\nUsage: " << argv[0]
<< " [--camera_index <1.Left | 2.Right> (default: 1)]"
<< " [--square_width <square width in meter (default: 0.12)] [--help] [-h]\n"
<< "\nExample using right camera and square size 0.1:\n"
<< " " << argv[0] << "--camera_index 2 --square_width 0.1\n"
<< std::endl;
return 0;
}
}
std::cout << "Use Realsense 2 grabber" << std::endl;
rs2::config config;
config.enable_stream(RS2_STREAM_FISHEYE, 1);
config.enable_stream(RS2_STREAM_FISHEYE, 2);
if(opt_camera_index == 1)
else
std::cout << "Read camera parameters from Realsense device" << std::endl;
std::cout << "Square width : " << opt_square_width << std::endl;
std::cout << cam << std::endl;
std::vector<vpDot2> dot(4);
std::vector<vpPoint> point;
std::vector<vpImagePoint> ip;
double L = opt_square_width / 2.;
point.push_back(
vpPoint(-L, -L, 0));
point.push_back(
vpPoint( L, -L, 0));
point.push_back(
vpPoint( L, L, 0));
point.push_back(
vpPoint(-L, L, 0));
#if defined(VISP_HAVE_X11)
#elif defined(VISP_HAVE_GDI)
#elif defined(VISP_HAVE_OPENCV)
#endif
bool quit = false;
bool apply_cv = false;
bool init_cv = true;
while (! quit) {
if(opt_camera_index == 1)
else
if (apply_cv) {
try {
ip = track(I, dot, init_cv);
computePose(point, ip, cam, init_cv, cMo);
if (init_cv)
init_cv = false;
{
std::stringstream ss;
ss << "Translation: " << std::setprecision(5) << pose[0] << " " << pose[1] << " " << pose[2] << " [m]";
ss.str("");
}
}
catch(...) {
std::cout << "Computer vision failure." << std::endl;
apply_cv = false;
init_cv = true;
}
}
if (apply_cv) {
} else {
}
quit = true;
}
apply_cv = true;
}
}
{
std::stringstream ss;
}
}
std::cout <<
"Catch an exception: " << e.
getMessage() << std::endl;
}
#elif !defined(VISP_HAVE_REALSENSE2)
(void)argc; (void)argv;
std::cout << "You do not realsense2 SDK functionality enabled..." << std::endl;
std::cout << "Tip:" << std::endl;
std::cout << "- Install librealsense2, configure again ViSP using cmake and build again this example" << std::endl;
return EXIT_SUCCESS;
#elif (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
(void)argc; (void)argv;
std::cout << "You do not build ViSP with c++11 or higher compiler flag" << std::endl;
std::cout << "Tip:" << std::endl;
std::cout << "- Configure ViSP again using cmake -DUSE_CXX_STANDARD=11, and build again this example" << std::endl;
#elif !(defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI) || defined(VISP_HAVE_OPENCV))
(void)argc; (void)argv;
std::cout << "Install a 3rd party dedicated to image display (X11, GDI, OpenCV), configure and build ViSP again to use this example" << std::endl;
#elif !(RS2_API_VERSION > ((2 * 10000) + (31 * 100) + 0))
(void)argc; (void)argv;
std::cout << "Install librealsense version > 2.31.0" << std::endl;
#endif
}