4 #include <visp3/core/vpIoTools.h>
5 #include <visp3/vision/vpHandEyeCalibration.h>
7 void usage(
const char *argv [],
int error)
9 std::cout <<
"Synopsis" << std::endl
10 <<
" " << argv[0] <<
" [--data-path <path>] [--help] [-h]" << std::endl
12 std::cout <<
"Description" << std::endl
13 <<
" --data-path <path> Path to the folder containing" << std::endl
14 <<
" pose_fPe_%d.yaml and pose_cPo_%d.yaml data files." << std::endl
15 <<
" Default: \"./\"" << std::endl
17 <<
" --fPe <generic name> Generic name of the yaml files" << std::endl
18 <<
" containing the pose of the end-effector expressed in the robot base" << std::endl
19 <<
" frame and located in the data path folder." << std::endl
20 <<
" Default: pose_fPe_%d.yaml" << std::endl
22 <<
" --cPo <generic name> Generic name of the yaml files" << std::endl
23 <<
" containing the pose of the calibration grid expressed in the camera" << std::endl
24 <<
" frame and located in the data path folder." << std::endl
25 <<
" Default: pose_cPo_%d.yaml" << std::endl
27 <<
" --output <filename> File in yaml format containing the pose of the camera" << std::endl
28 <<
" in the end-effector frame. Data are saved as a pose vector" << std::endl
29 <<
" with first the 3 translations along X,Y,Z in [m]" << std::endl
30 <<
" and then the 3 rotations in axis-angle representation (thetaU) in [rad]." << std::endl
31 <<
" Default: eMc.yaml" << std::endl
33 <<
" --help, -h Print this helper message." << std::endl
36 std::cout <<
"Error" << std::endl
38 <<
"Unsupported parameter " << argv[error] << std::endl;
42 int main(
int argc,
const char *argv [])
44 std::string opt_data_path =
"./";
45 std::string opt_fPe_files =
"pose_fPe_%d.yaml";
46 std::string opt_cPo_files =
"pose_cPo_%d.yaml";
47 std::string opt_eMc_file =
"eMc.yaml";
49 for (
int i = 1; i < argc; i++) {
50 if (std::string(argv[i]) ==
"--data-path" && i + 1 < argc) {
51 opt_data_path = std::string(argv[i + 1]);
54 else if (std::string(argv[i]) ==
"--fPe" && i + 1 < argc) {
55 opt_fPe_files = std::string(argv[i + 1]);
58 else if (std::string(argv[i]) ==
"--cPo" && i + 1 < argc) {
59 opt_cPo_files = std::string(argv[i + 1]);
62 else if (std::string(argv[i]) ==
"--output" && i + 1 < argc) {
63 opt_eMc_file = std::string(argv[i + 1]);
66 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
76 std::vector<vpHomogeneousMatrix> cMo;
77 std::vector<vpHomogeneousMatrix> wMe;
80 std::map<long, std::string> map_fPe_files;
81 std::map<long, std::string> map_cPo_files;
83 for (
unsigned int i = 0; i < files.size(); i++) {
86 if (index_fPe != -1) {
87 map_fPe_files[index_fPe] = files[i];
89 if (index_cPo != -1) {
90 map_cPo_files[index_cPo] = files[i];
94 if (map_fPe_files.size() == 0) {
95 std::cout <<
"No " << opt_fPe_files
96 <<
" files found. Use --data-path <path> or --fPe <generic name> be able to read your data." << std::endl;
99 if (map_cPo_files.size() == 0) {
100 std::cout <<
"No " << opt_cPo_files
101 <<
" files found. Use --data-path <path> or --cPo <generic name> be able to read your data." << std::endl;
105 for (std::map<long, std::string>::const_iterator it_fPe = map_fPe_files.begin(); it_fPe != map_fPe_files.end();
108 std::map<long, std::string>::const_iterator it_cPo = map_cPo_files.find(it_fPe->first);
109 if (it_cPo != map_cPo_files.end()) {
111 if (wPe.
loadYAML(file_fPe, wPe) ==
false) {
112 std::cout <<
"Unable to read data from " << file_fPe <<
". Skip data" << std::endl;
118 if (cPo.
loadYAML(file_cPo, cPo) ==
false) {
119 std::cout <<
"Unable to read data from " << file_cPo <<
". Skip data" << std::endl;
122 std::cout <<
"Use data from " << opt_data_path <<
"/" << file_fPe <<
" and from " << file_cPo << std::endl;
128 if (wMe.size() < 3) {
129 std::cout <<
"Not enough data pairs found." << std::endl;
136 std::cout << std::endl <<
"** Hand-eye calibration succeed" << std::endl;
137 std::cout << std::endl <<
"** Hand-eye (eMc) transformation estimated:" << std::endl;
138 std::cout << eMc << std::endl;
139 std::cout <<
"** Corresponding pose vector [tx ty tz tux tuy tuz] in [m] and [rad]: " <<
vpPoseVector(eMc).
t() << std::endl;
142 std::cout << std::endl <<
"** Translation [m]: " << eMc[0][3] <<
" " << eMc[1][3] <<
" " << eMc[2][3] << std::endl;
143 std::cout <<
"** Rotation (theta-u representation) [rad]: " << erc.
t() << std::endl;
144 std::cout <<
"** Rotation (theta-u representation) [deg]: " <<
vpMath::deg(erc[0]) <<
" " <<
vpMath::deg(erc[1])
147 std::cout <<
"** Rotation (quaternion representation) [rad]: " << quaternion.t() << std::endl;
149 std::cout <<
"** Rotation (r-x-y-z representation) [rad]: " << rxyz.t() << std::endl;
150 std::cout <<
"** Rotation (r-x-y-z representation) [deg]: " <<
vpMath::deg(rxyz).t() << std::endl;
154 std::cout << std::endl <<
"Save transformation matrix eMc as an homogeneous matrix in: " << name_we << std::endl;
155 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
156 std::ofstream file_eMc(name_we);
158 std::ofstream file_eMc(name_we.c_str());
164 std::cout <<
"Save transformation matrix eMc as a vpPoseVector in : " << output_filename << std::endl;
165 pose_vec.saveYAML(output_filename, pose_vec);
168 std::cout << std::endl <<
"** Hand-eye calibration failed" << std::endl;
169 std::cout << std::endl <<
"Check your input data and ensure they are covering the half sphere over the chessboard." << std::endl;
170 std::cout << std::endl <<
"See https://visp-doc.inria.fr/doxygen/visp-daily/tutorial-calibration-extrinsic.html" << std::endl;
vpArray2D< Type > t() const
Compute the transpose of the array.
static bool loadYAML(const std::string &filename, vpArray2D< Type > &A, char *header=NULL)
static int calibrate(const std::vector< vpHomogeneousMatrix > &cMo, const std::vector< vpHomogeneousMatrix > &rMe, vpHomogeneousMatrix &eMc)
Implementation of an homogeneous matrix and operations on such kind of matrices.
vpRotationMatrix getRotationMatrix() const
void save(std::ofstream &f) const
static double deg(double rad)
Implementation of a pose vector and operations on poses.
Implementation of a rotation vector as quaternion angle minimal representation.
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as axis-angle minimal representation.