Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
tutorial-chessboard-pose.cpp
#include <iostream>
#include <visp3/core/vpConfig.h>
#if defined(VISP_HAVE_OPENCV) && defined(HAVE_OPENCV_CALIB3D)
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpPixelMeterConversion.h>
#include <visp3/core/vpPoint.h>
#include <visp3/core/vpXmlParserCamera.h>
#include <visp3/gui/vpDisplayD3D.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayOpenCV.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpVideoReader.h>
#include <visp3/vision/vpPose.h>
namespace
{
void calcChessboardCorners(int width, int height, double squareSize, std::vector<vpPoint> &corners)
{
corners.resize(0);
for (int i = 0; i < height; i++) {
for (int j = 0; j < width; j++) {
vpPoint pt;
pt.set_oX(j * squareSize);
pt.set_oY(i * squareSize);
pt.set_oZ(0.0);
corners.push_back(pt);
}
}
}
void usage(const char **argv, int error)
{
std::cout << "Synopsis" << std::endl
<< " " << argv[0] << " [-w <chessboard width>] [-h <chessboard height>]"
<< " [--square_size <square size in meter>]"
<< " [--input <input images path>]"
<< " [--intrinsic <Camera intrinsic parameters xml file>]"
<< " [--camera_name <Camera name in the xml intrinsic file>]"
<< " [--output <camera pose files>]"
<< " [--help] [-h]" << std::endl
<< std::endl;
std::cout << "Description" << std::endl
<< " -w <chessboard width> Chessboard width." << std::endl
<< " Default: 9." << std::endl
<< std::endl
<< " -h <chessboard height> Chessboard height." << std::endl
<< " Default: 6." << std::endl
<< std::endl
<< " --square_size <square size in meter> Chessboard square size in [m]." << std::endl
<< " Default: 0.03." << std::endl
<< std::endl
<< " --input <input images path> Generic name of the images to process." << std::endl
<< " Example: \"image-%02d.png\"." << std::endl
<< std::endl
<< " --intrinsic <Camera intrinsic parameters xml file> XML file that contains" << std::endl
<< " camera parameters. " << std::endl
<< " Default: \"camera.xml\"." << std::endl
<< std::endl
<< " --camera_name <Camera name in the xml intrinsic file> Camera name in the XML file." << std::endl
<< " Default: \"Camera\"." << std::endl
<< std::endl
<< " --output <camera pose files> Generic name of the yaml files that contains the camera poses."
<< std::endl
<< " Example: \"pose_cPo_%d.yaml\"." << std::endl
<< std::endl
<< " --help, -h Print this helper message." << std::endl
<< std::endl;
if (error) {
std::cout << "Error" << std::endl
<< " "
<< "Unsupported parameter " << argv[error] << std::endl;
}
}
} // namespace
int main(int argc, const char **argv)
{
int opt_chessboard_width = 9, opt_chessboard_height = 6;
double opt_chessboard_square_size = 0.03;
std::string opt_input_img_files = "";
std::string opt_intrinsic_file = "camera.xml";
std::string opt_camera_name = "Camera";
std::string opt_output_pose_files = "pose_cPo_%d.yaml";
for (int i = 1; i < argc; i++) {
if (std::string(argv[i]) == "-w" && i + 1 < argc) {
opt_chessboard_width = atoi(argv[i + 1]);
i++;
}
else if (std::string(argv[i]) == "-h" && i + 1 < argc) {
opt_chessboard_height = atoi(argv[i + 1]);
i++;
}
else if (std::string(argv[i]) == "--square_size" && i + 1 < argc) {
opt_chessboard_square_size = atof(argv[i + 1]);
i++;
}
else if (std::string(argv[i]) == "--input" && i + 1 < argc) {
opt_input_img_files = std::string(argv[i + 1]);
i++;
}
else if (std::string(argv[i]) == "--intrinsic" && i + 1 < argc) {
opt_intrinsic_file = std::string(argv[i + 1]);
i++;
}
else if (std::string(argv[i]) == "--output" && i + 1 < argc) {
opt_output_pose_files = std::string(argv[i + 1]);
i++;
}
else if (std::string(argv[i]) == "--camera_name" && i + 1 < argc) {
opt_camera_name = std::string(argv[i + 1]);
i++;
}
else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
usage(argv, 0);
return EXIT_SUCCESS;
}
else {
usage(argv, i);
return EXIT_FAILURE;
}
}
if (!vpIoTools::checkFilename(opt_intrinsic_file)) {
std::cout << "Camera parameters file " << opt_intrinsic_file << " doesn't exist." << std::endl;
std::cout << "Use --help option to see how to set its location..." << std::endl;
return EXIT_SUCCESS;
}
std::cout << "Parameters:" << std::endl;
std::cout << " chessboard width : " << opt_chessboard_width << std::endl;
std::cout << " chessboard height : " << opt_chessboard_height << std::endl;
std::cout << " chessboard square size [m] : " << opt_chessboard_square_size << std::endl;
std::cout << " input images location : " << opt_input_img_files << std::endl;
std::cout << " camera param file name [.xml]: " << opt_intrinsic_file << std::endl;
std::cout << " camera name : " << opt_camera_name << std::endl;
std::cout << " output camera poses : " << opt_output_pose_files << std::endl << std::endl;
if (opt_input_img_files.empty()) {
std::cout << "Input images location empty." << std::endl;
std::cout << "Use --help option to see how to set input image location..." << std::endl;
return EXIT_FAILURE;
}
try {
vpVideoReader reader;
reader.setFileName(opt_input_img_files);
reader.open(I);
#ifdef VISP_HAVE_X11
vpDisplayX d(I);
#elif defined(VISP_HAVE_GDI)
#elif defined(HAVE_OPENCV_HIGHGUI)
#endif
std::vector<vpPoint> corners_pts;
calcChessboardCorners(opt_chessboard_width, opt_chessboard_height, opt_chessboard_square_size, corners_pts);
if (!opt_intrinsic_file.empty() && !opt_camera_name.empty()) {
if (parser.parse(cam, opt_intrinsic_file, opt_camera_name, vpCameraParameters::perspectiveProjWithDistortion) !=
std::cout << "Unable to parse parameters with distortion for camera \"" << opt_camera_name << "\" from "
<< opt_intrinsic_file << " file" << std::endl;
std::cout << "Attempt to find parameters without distortion" << std::endl;
if (parser.parse(cam, opt_intrinsic_file, opt_camera_name,
std::cout << "Unable to parse parameters without distortion for camera \"" << opt_camera_name << "\" from "
<< opt_intrinsic_file << " file" << std::endl;
return EXIT_FAILURE;
}
}
}
std::cout << "Camera parameters used to compute the pose:\n" << cam << std::endl;
bool quit = false;
do {
reader.acquire(I);
cv::Mat matImg;
vpDisplay::displayText(I, 20, 20, "Right click to quit.", vpColor::red);
cv::Size chessboardSize(opt_chessboard_width, opt_chessboard_height);
std::vector<cv::Point2f> corners2D;
bool found = cv::findChessboardCorners(matImg, chessboardSize, corners2D,
#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
cv::CALIB_CB_ADAPTIVE_THRESH | cv::CALIB_CB_FAST_CHECK |
cv::CALIB_CB_NORMALIZE_IMAGE);
#else
CV_CALIB_CB_ADAPTIVE_THRESH | CV_CALIB_CB_FAST_CHECK |
CV_CALIB_CB_NORMALIZE_IMAGE);
#endif
if (found) {
cv::Mat matImg_gray;
cv::cvtColor(matImg, matImg_gray, cv::COLOR_BGR2GRAY);
cv::cornerSubPix(matImg_gray, corners2D, cv::Size(11, 11), cv::Size(-1, -1),
#if (VISP_HAVE_OPENCV_VERSION >= 0x030000)
cv::TermCriteria(cv::TermCriteria::EPS + cv::TermCriteria::COUNT, 30, 0.1));
#else
cv::TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1));
#endif
for (size_t i = 0; i < corners_pts.size(); i++) {
vpImagePoint imPt(corners2D[i].y, corners2D[i].x);
double x = 0.0, y = 0.0;
corners_pts[i].set_x(x);
corners_pts[i].set_y(y);
}
vpPose pose;
pose.addPoints(corners_pts);
std::cerr << "Problem when computing final pose using VVS" << std::endl;
return EXIT_FAILURE;
}
cv::drawChessboardCorners(matImg, chessboardSize, corners2D, found);
}
vpDisplay::displayText(I, 20, 20, "Left click for the next image, right click to quit.", vpColor::red);
if (found)
vpDisplay::displayFrame(I, cMo, cam, 0.05, vpColor::none, 3);
if (found) {
vpPoseVector pose_vec(cMo);
char name[FILENAME_MAX];
snprintf(name, FILENAME_MAX, opt_output_pose_files.c_str(), reader.getFrameIndex());
std::string s = name;
std::cout << "Save " << s << std::endl;
pose_vec.saveYAML(s, pose_vec);
}
if (vpDisplay::getClick(I, button, true)) {
switch (button) {
quit = true;
break;
default:
break;
}
}
} while (!quit && !reader.end());
}
catch (const vpException &e) {
std::cout << "Catch an exception: " << e.getMessage() << std::endl;
}
return EXIT_SUCCESS;
}
#else
int main()
{
std::cerr << "OpenCV 2.3.0 or higher is requested to run the calibration." << std::endl;
return EXIT_SUCCESS;
}
#endif
Generic class defining intrinsic camera parameters.
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
static const vpColor red
Definition vpColor.h:211
static const vpColor none
Definition vpColor.h:223
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...
Definition vpDisplayX.h:132
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 setTitle(const vpImage< unsigned char > &I, const std::string &windowtitle)
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.
Definition vpException.h:59
const char * getMessage() const
Implementation of an homogeneous matrix and operations on such kind of matrices.
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 of the vpImage class member functions.
Definition vpImage.h:135
static bool checkFilename(const std::string &filename)
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
Class that defines a 3D point in the object frame and allows forward projection of a 3D point in the ...
Definition vpPoint.h:77
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition vpPoint.cpp:501
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition vpPoint.cpp:503
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition vpPoint.cpp:499
Implementation of a pose vector and operations on poses.
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition vpPose.h:81
@ DEMENTHON_LAGRANGE_VIRTUAL_VS
Definition vpPose.h:102
void addPoints(const std::vector< vpPoint > &lP)
Definition vpPose.cpp:155
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition vpPose.cpp:469
Class that enables to manipulate easily a video file or a sequence of images. As it inherits from the...
void acquire(vpImage< vpRGBa > &I)
void open(vpImage< vpRGBa > &I)
void setFileName(const std::string &filename)
std::string getFrameName() const
long getFrameIndex() const
XML parser to load and save intrinsic camera parameters.
int parse(vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, const vpCameraParameters::vpCameraParametersProjType &projModel, unsigned int image_width=0, unsigned int image_height=0, bool verbose=true)