44 #include <visp3/core/vpCameraParameters.h>
45 #include <visp3/core/vpDebug.h>
46 #include <visp3/core/vpException.h>
47 #include <visp3/core/vpMath.h>
48 #include <visp3/core/vpPixelMeterConversion.h>
60 const double &rho_p,
const double &theta_p,
61 double &rho_m,
double &theta_m)
63 double co = cos(theta_p);
64 double si = sin(theta_p);
71 theta_m = atan2(si * cam.py, co * cam.px);
72 rho_m = (rho_p - cam.u0 * co - cam.v0 * si) / sqrt(d);
86 unsigned int order,
const vpMatrix &moment_pixel,
94 for (
unsigned int k = 0; k < order; k++)
96 for (
unsigned int p = 0; p < order; p++)
97 for (
unsigned int q = 0; q < order; q++)
101 for (
unsigned int r = 0; r <= p; r++)
102 for (
unsigned int t = 0; t <= q; t++)
105 pow(xc, (
int)(p - r)) * pow(yc, (
int)(q - t)) * moment_pixel[r][t];
110 for (
unsigned int k = 0; k < order; k++)
111 for (
unsigned int p = 0; p < order; p++)
112 for (
unsigned int q = 0; q < order; q++)
114 m[p][q] *= pow(cam.inv_px, (
int)(1 + p)) * pow(cam.inv_py, (
int)(1 + q));
117 for (
unsigned int k = 0; k < order; k++)
118 for (
unsigned int p = 0; p < order; p++)
119 for (
unsigned int q = 0; q < order; q++)
121 moment_meter[p][q] = m[p][q];
125 #if VISP_HAVE_OPENCV_VERSION >= 0x020300
137 const double &rho_p,
const double &theta_p,
138 double &rho_m,
double &theta_m)
140 double co = cos(theta_p);
141 double si = sin(theta_p);
142 double px = cameraMatrix.at<
double>(0,0);
143 double py = cameraMatrix.at<
double>(1,1);
144 double u0 = cameraMatrix.at<
double>(0,2);
145 double v0 = cameraMatrix.at<
double>(1,2);
149 if (fabs(d) < 1e-6) {
153 theta_m = atan2(si * py, co * px);
154 rho_m = (rho_p - u0 * co - v0 * si) / sqrt(d);
168 unsigned int order,
const vpMatrix &moment_pixel,
171 double inv_px = 1. / cameraMatrix.at<
double>(0,0);
172 double inv_py = 1. / cameraMatrix.at<
double>(1,1);
173 double u0 = cameraMatrix.at<
double>(0,2);
174 double v0 = cameraMatrix.at<
double>(1,2);
180 for (
unsigned int k = 0; k < order; k++)
182 for (
unsigned int p = 0; p < order; p++)
183 for (
unsigned int q = 0; q < order; q++)
187 for (
unsigned int r = 0; r <= p; r++)
188 for (
unsigned int t = 0; t <= q; t++)
191 pow(xc,
static_cast<int>(p - r)) * pow(yc,
static_cast<int>(q - t)) * moment_pixel[r][t];
196 for (
unsigned int k = 0; k < order; k++)
197 for (
unsigned int p = 0; p < order; p++)
198 for (
unsigned int q = 0; q < order; q++)
200 m[p][q] *= pow(inv_px,
static_cast<int>(1 + p)) * pow(inv_py,
static_cast<int>(1 + q));
203 for (
unsigned int k = 0; k < order; k++)
204 for (
unsigned int p = 0; p < order; p++)
205 for (
unsigned int q = 0; q < order; q++)
207 moment_meter[p][q] = m[p][q];
226 const double &u,
const double &v,
double &x,
double &y)
228 std::vector<cv::Point2d> imagePoints_vec;
229 imagePoints_vec.push_back(cv::Point2d(u, v));
230 std::vector<cv::Point2d> objectPoints_vec;
231 cv::undistortPoints(imagePoints_vec, objectPoints_vec, cameraMatrix, distCoeffs);
232 x = objectPoints_vec[0].x;
233 y = objectPoints_vec[0].y;
252 std::vector<cv::Point2d> imagePoints_vec;
253 imagePoints_vec.push_back(cv::Point2d(iP.
get_u(), iP.
get_v()));
254 std::vector<cv::Point2d> objectPoints_vec;
255 cv::undistortPoints(imagePoints_vec, objectPoints_vec, cameraMatrix, distCoeffs);
256 x = objectPoints_vec[0].x;
257 y = objectPoints_vec[0].y;