Visual Servoing Platform  version 3.5.1 under development (2022-10-03)
tutorial-chessboard-pose.cpp
#include <iostream>
#include <visp3/core/vpConfig.h>
#if VISP_HAVE_OPENCV_VERSION >= 0x020300
#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_cMo-%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 VISP_HAVE_OPENCV
#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 distorsion for camera \"" << opt_camera_name << "\" from "
<< opt_intrinsic_file << " file" << std::endl;
std::cout << "Attempt to find parameters without distorsion" << std::endl;
if (parser.parse(cam, opt_intrinsic_file, opt_camera_name,
std::cout << "Unable to parse parameters without distorsion 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);
vpHomogeneousMatrix cMo_dementhon, cMo_lagrange;
double r_dementhon = std::numeric_limits<double>::max(), r_lagrange = std::numeric_limits<double>::max();
bool pose_dementhon = pose.computePose(vpPose::DEMENTHON, cMo_dementhon);
if (pose_dementhon)
r_dementhon = pose.computeResidual(cMo_dementhon);
bool pose_lagrange = pose.computePose(vpPose::LAGRANGE, cMo_lagrange);
if (pose_lagrange)
r_lagrange = pose.computeResidual(cMo_lagrange);
cMo = (r_dementhon < r_lagrange) ? cMo_dementhon : cMo_lagrange;
if (!pose.computePose(vpPose::VIRTUAL_VS, cMo)) {
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];
sprintf(name, 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.
static const vpColor red
Definition: vpColor.h:217
static const vpColor none
Definition: vpColor.h:229
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:129
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:135
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void setTitle(const vpImage< unsigned char > &I, const std::string &windowtitle)
static void flush(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))
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
error that can be emited by ViSP classes.
Definition: vpException.h:72
const char * getMessage() const
Definition: vpException.cpp:87
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: vpImagePoint.h:89
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:797
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:82
void set_oY(double oY)
Set the point oY coordinate in the object frame.
Definition: vpPoint.cpp:504
void set_oZ(double oZ)
Set the point oZ coordinate in the object frame.
Definition: vpPoint.cpp:506
void set_oX(double oX)
Set the point oX coordinate in the object frame.
Definition: vpPoint.cpp:502
Implementation of a pose vector and operations on poses.
Definition: vpPoseVector.h:152
Class used for pose computation from N points (pose from point only). Some of the algorithms implemen...
Definition: vpPose.h:90
@ DEMENTHON
Definition: vpPose.h:95
@ VIRTUAL_VS
Definition: vpPose.h:104
@ LAGRANGE
Definition: vpPose.h:94
void addPoints(const std::vector< vpPoint > &lP)
Definition: vpPose.cpp:163
double computeResidual(const vpHomogeneousMatrix &cMo) const
Compute and return the sum of squared residuals expressed in meter^2 for the pose matrix cMo.
Definition: vpPose.cpp:335
bool computePose(vpPoseMethodType method, vpHomogeneousMatrix &cMo, bool(*func)(const vpHomogeneousMatrix &)=NULL)
Definition: vpPose.cpp:373
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)