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;
Implementation of a matrix and operations on matrices.
error that can be emited by ViSP classes.
static void convertPoint(const vpCameraParameters &cam, const double &u, const double &v, double &x, double &y)
static double sqr(double x)
Generic class defining intrinsic camera parameters.
static void convertMoment(const vpCameraParameters &cam, unsigned int order, const vpMatrix &moment_pixel, vpMatrix &moment_meter)
static long double comb(unsigned int n, unsigned int p)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static void convertLine(const vpCameraParameters &cam, const double &rho_p, const double &theta_p, double &rho_m, double &theta_m)