API tools faq
paste
Guest User

Untitled

a guest
Apr 22nd, 2018
74
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 15.86 KB | None | 0 0
raw download clone embed print report
  1. #ifndef UNDISTORTER_HPP_
  2. #define UNDISTORTER_HPP_
  3.  
  4. #include <boost/make_shared.hpp>
  5. #include <boost/shared_ptr.hpp>
  6. #include <Eigen/Core>
  7. #include <Eigen/Dense>
  8. #include <opencv2/core/core.hpp>
  9. #include <opencv2/imgproc/imgproc.hpp>
  10. #include <opencv2/calib3d/calib3d.hpp>
  11. #include <opencv2/core/eigen.hpp>
  12.  
  13.  
  14. namespace undistorter {
  15.  
  16. //////////////////////////////////////////////////////
  17. // Distortion models
  18. //////////////////////////////////////////////////////
  19. class DistortionModel {
  20. public:
  21. virtual void distort(Eigen::Vector2d& y) const = 0;
  22. virtual void undistort(Eigen::Vector2d& y) const = 0;
  23.  
  24. typedef boost::shared_ptr<DistortionModel> Ptr;
  25. typedef boost::shared_ptr<const DistortionModel> ConstPtr;
  26. };
  27.  
  28. class NullDistortion : public DistortionModel
  29. {
  30. public:
  31. typedef boost::shared_ptr<NullDistortion> Ptr;
  32. typedef boost::shared_ptr<const NullDistortion> ConstPtr;
  33.  
  34. static Ptr create()
  35. {
  36. return boost::make_shared<NullDistortion>();
  37. }
  38.  
  39. NullDistortion() {};
  40. virtual ~NullDistortion() {};
  41.  
  42. void distort(Eigen::Vector2d& y) const {};
  43. void undistort(Eigen::Vector2d& y) const {};
  44. };
  45.  
  46.  
  47. class EquidistantDistortion : public DistortionModel
  48. {
  49. public:
  50. typedef boost::shared_ptr<EquidistantDistortion> Ptr;
  51. typedef boost::shared_ptr<const EquidistantDistortion> ConstPtr;
  52.  
  53. static Ptr create(const Eigen::Vector4d& distCoeffs)
  54. {
  55. return boost::make_shared<EquidistantDistortion>(distCoeffs);
  56. }
  57.  
  58. EquidistantDistortion(const Eigen::Vector4d& distCoeffs)
  59. : _distCoeffs(distCoeffs) {};
  60.  
  61. virtual ~EquidistantDistortion() {};
  62.  
  63. void distort(Eigen::Vector2d& y) const
  64. {
  65. double r, theta, theta2, theta4, theta6, theta8, thetad, scaling;
  66.  
  67. r = sqrt(y[0]*y[0] + y[1]*y[1]);
  68. theta = atan(r);
  69. theta2 = theta * theta;
  70. theta4 = theta2 * theta2;
  71. theta6 = theta4 * theta2;
  72. theta8 = theta4 * theta4;
  73. thetad = theta * (1 + _distCoeffs[0] * theta2 + _distCoeffs[1] * theta4 + _distCoeffs[2] * theta6 + _distCoeffs[3] * theta8);
  74. scaling = (r > 1e-8) ? thetad / r : 1.0;
  75. y[0] *= scaling;
  76. y[1] *= scaling;
  77. };
  78.  
  79. void undistort(Eigen::Vector2d& y) const
  80. {
  81. double theta, theta2, theta4, theta6, theta8, thetad, scaling;
  82.  
  83. thetad = sqrt(y[0] * y[0] + y[1] * y[1]);
  84. theta = thetad; // initial guess
  85. for (int i = 20; i > 0; i--) {
  86. theta2 = theta * theta;
  87. theta4 = theta2 * theta2;
  88. theta6 = theta4 * theta2;
  89. theta8 = theta4 * theta4;
  90. theta = thetad / (1 + _distCoeffs[0] * theta2 + _distCoeffs[1] * theta4 + _distCoeffs[2] * theta6 + _distCoeffs[3] * theta8);
  91. }
  92.  
  93. scaling = tan(theta) / thetad;
  94. y[0] *= scaling;
  95. y[1] *= scaling;
  96. };
  97.  
  98. private:
  99. const Eigen::Vector4d _distCoeffs;
  100. };
  101. class RadialTangentialDistortion : public DistortionModel
  102. {
  103. public:
  104. typedef boost::shared_ptr<RadialTangentialDistortion> Ptr;
  105. typedef boost::shared_ptr<const RadialTangentialDistortion> ConstPtr;
  106.  
  107. static Ptr create(const Eigen::Vector4d& distCoeffs)
  108. {
  109. return boost::make_shared<RadialTangentialDistortion>(distCoeffs);
  110. }
  111.  
  112. RadialTangentialDistortion(const Eigen::Vector4d& distCoeffs)
  113. : _distCoeffs(distCoeffs) {};
  114.  
  115. virtual ~RadialTangentialDistortion() {};
  116.  
  117. void distort(Eigen::Vector2d& y) const
  118. {
  119. double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;
  120.  
  121. mx2_u = y[0] * y[0];
  122. my2_u = y[1] * y[1];
  123. mxy_u = y[0] * y[1];
  124. rho2_u = mx2_u + my2_u;
  125. rad_dist_u = _distCoeffs[0] * rho2_u + _distCoeffs[1] * rho2_u * rho2_u;
  126. y[0] += y[0] * rad_dist_u + 2.0 * _distCoeffs[2] * mxy_u + _distCoeffs[3] * (rho2_u + 2.0 * mx2_u);
  127. y[1] += y[1] * rad_dist_u + 2.0 * _distCoeffs[3] * mxy_u + _distCoeffs[2] * (rho2_u + 2.0 * my2_u);
  128. };
  129.  
  130. void undistort(Eigen::Vector2d& y) const
  131. {
  132. const int n = 5;
  133. Eigen::Vector2d ybar = y;
  134. Eigen::Matrix2d J;
  135.  
  136. Eigen::Vector2d y_tmp;
  137.  
  138. for (int i = 0; i < n; i++)
  139. {
  140. y_tmp = ybar;
  141.  
  142. double mx2_u, my2_u, mxy_u, rho2_u, rad_dist_u;
  143. mx2_u = y_tmp[0] * y_tmp[0];
  144. my2_u = y_tmp[1] * y_tmp[1];
  145. mxy_u = y_tmp[0] * y_tmp[1];
  146. rho2_u = mx2_u + my2_u;
  147. rad_dist_u = _distCoeffs[0] * rho2_u + _distCoeffs[1] * rho2_u * rho2_u;
  148.  
  149. J(0, 0) = 1 + rad_dist_u + _distCoeffs[0] * 2.0 * mx2_u + _distCoeffs[1] * rho2_u * 4 * mx2_u
  150. + 2.0 * _distCoeffs[2] * y_tmp[1] + 6 * _distCoeffs[3] * y_tmp[0];
  151. J(1, 0) = _distCoeffs[0] * 2.0 * y_tmp[0] * y_tmp[1] + _distCoeffs[1] * 4 * rho2_u * y_tmp[0] * y_tmp[1]
  152. + _distCoeffs[2] * 2.0 * y_tmp[0] + 2.0 * _distCoeffs[3] * y_tmp[1];
  153. J(0, 1) = J(1, 0);
  154. J(1, 1) = 1 + rad_dist_u + _distCoeffs[0] * 2.0 * my2_u + _distCoeffs[1] * rho2_u * 4 * my2_u
  155. + 6 * _distCoeffs[2] * y[1] + 2.0 * _distCoeffs[3] * y[0];
  156. y_tmp[0] += y_tmp[0] * rad_dist_u + 2.0 * _distCoeffs[2] * mxy_u + _distCoeffs[3] * (rho2_u + 2.0 * mx2_u);
  157. y_tmp[1] += y_tmp[1] * rad_dist_u + 2.0 * _distCoeffs[3] * mxy_u + _distCoeffs[2] * (rho2_u + 2.0 * my2_u);
  158.  
  159. Eigen::Vector2d e(y - y_tmp);
  160. Eigen::Vector2d du = (J.transpose() * J).inverse() * J.transpose() * e;
  161. ybar += du;
  162.  
  163. if (e.dot(e) < 1e-15)
  164. break;
  165. }
  166. y = ybar;
  167. };
  168.  
  169. private:
  170. //_distCoeffs: k1, k2, p1, p2
  171. const Eigen::Vector4d _distCoeffs;
  172. };
  173.  
  174.  
  175. //////////////////////////////////////////////////////
  176. // Camera Model
  177. //////////////////////////////////////////////////////
  178. struct PinholeGeometry {
  179. PinholeGeometry()
  180. : focallength(Eigen::Vector2d(0,0)),
  181. principalpoint(Eigen::Vector2d(0,0)),
  182. resolution(Eigen::Vector2i(0,0)),
  183. distortion(NullDistortion::create())
  184. {};
  185.  
  186. PinholeGeometry(const Eigen::Vector2d& focallength,
  187. const Eigen::Vector2d& principalpoint,
  188. const Eigen::Vector2i& resolution,
  189. DistortionModel::Ptr distortion)
  190. : focallength(focallength),
  191. principalpoint(principalpoint),
  192. resolution(resolution),
  193. distortion(distortion)
  194. {};
  195.  
  196. PinholeGeometry(const Eigen::Matrix3d& cameraMatrix,
  197. const Eigen::Vector2i& resolution,
  198. DistortionModel::Ptr distortion)
  199. : resolution(resolution),
  200. distortion(distortion)
  201. {
  202. focallength[0] = cameraMatrix(0, 0);
  203. focallength[1] = cameraMatrix(1, 1);
  204. principalpoint[0] = cameraMatrix(0, 2);
  205. principalpoint[1] = cameraMatrix(1, 2);
  206. };
  207.  
  208. Eigen::Matrix3d getCameraMatrix() const
  209. {
  210. Eigen::Matrix3d cameraMatrix = Eigen::Matrix3d::Zero();
  211. cameraMatrix(0,0) = focallength[0];
  212. cameraMatrix(1,1) = focallength[1];
  213. cameraMatrix(0,2) = principalpoint[0];
  214. cameraMatrix(1,2) = principalpoint[1];
  215. cameraMatrix(2,2) = 1.0;
  216. return cameraMatrix;
  217. }
  218.  
  219. //camera parameters
  220. Eigen::Vector2d focallength;
  221. Eigen::Vector2d principalpoint;
  222. Eigen::Vector2i resolution;
  223.  
  224. //distortion
  225. DistortionModel::Ptr distortion;
  226. };
  227.  
  228. //////////////////////////////////////////////////////
  229. // Custom OpenCV functions
  230. //////////////////////////////////////////////////////
  231. namespace cv_helper {
  232. using namespace cv;
  233. ///
  234. //calculates the inner(min)/outer(max) rectangle on the undistorted image
  235. //
  236. // INPUT:
  237. // camera_geometry: camera geometry (distortion and intrinsics used)
  238. // imgSize: The original image size.
  239. // OUTPUT:
  240. // inner: inner rectangle (all pixels valid)
  241. // outer: outer rectangle (no pixels lost)
  242. ///
  243. static void icvGetRectangles(const PinholeGeometry& camera_geometry,
  244. CvSize imgSize, cv::Rect_<float>& inner,
  245. cv::Rect_<float>& outer) {
  246. const int N = 9;
  247. int x, y, k;
  248. cv::Ptr<CvMat> _pts(cvCreateMat(1, N * N, CV_32FC2));
  249. CvPoint2D32f* pts = (CvPoint2D32f*) (_pts->data.ptr);
  250.  
  251. for (y = k = 0; y < N; y++) {
  252. for (x = 0; x < N; x++) {
  253. Eigen::Vector2d point(x * imgSize.width / (N - 1),
  254. y * imgSize.height / (N - 1));
  255.  
  256. //normalize
  257. Eigen::Matrix3d cameraMatrix = camera_geometry.getCameraMatrix();
  258. double cu = cameraMatrix(0, 2), cv = cameraMatrix(1, 2);
  259. double fu = cameraMatrix(0, 0), fv = cameraMatrix(1, 1);
  260. point(0) = (point(0) - cu) / fu;
  261. point(1) = (point(1) - cv) / fv;
  262.  
  263. //undistort
  264. camera_geometry.distortion->undistort(point);
  265.  
  266. pts[k++] = cvPoint2D32f((float) point[0], (float) point[1]);
  267. }
  268. }
  269.  
  270. float iX0 = -FLT_MAX, iX1 = FLT_MAX, iY0 = -FLT_MAX, iY1 = FLT_MAX;
  271. float oX0 = FLT_MAX, oX1 = -FLT_MAX, oY0 = FLT_MAX, oY1 = -FLT_MAX;
  272. // find the inscribed rectangle.
  273. // the code will likely not work with extreme rotation matrices (R) (>45%)
  274. for (y = k = 0; y < N; y++)
  275. for (x = 0; x < N; x++) {
  276. CvPoint2D32f p = pts[k++];
  277. oX0 = MIN(oX0, p.x);
  278. oX1 = MAX(oX1, p.x);
  279. oY0 = MIN(oY0, p.y);
  280. oY1 = MAX(oY1, p.y);
  281.  
  282. if (x == 0)
  283. iX0 = MAX(iX0, p.x);
  284. if (x == N - 1)
  285. iX1 = MIN(iX1, p.x);
  286. if (y == 0)
  287. iY0 = MAX(iY0, p.y);
  288. if (y == N - 1)
  289. iY1 = MIN(iY1, p.y);
  290. }
  291. inner = cv::Rect_<float>(iX0, iY0, iX1 - iX0, iY1 - iY0);
  292. outer = cv::Rect_<float>(oX0, oY0, oX1 - oX0, oY1 - oY0);
  293. }
  294.  
  295. ///
  296. //Returns the new camera matrix based on the free scaling parameter.
  297. //
  298. // INPUT:
  299. // camera_geometry: camera geometry (distortion and intrinsics used)
  300. // imgSize: The original image size.
  301. // alpha: Free scaling parameter between 0 (when all the pixels in the undistorted image will be valid)
  302. // and 1 (when all the source image pixels will be retained in the undistorted image)
  303. // newImgSize: Image size after rectification. By default it will be set to imageSize.
  304. // RETURNS: The output new camera matrix.
  305. ///
  306. Eigen::Matrix3d getOptimalNewCameraMatrix(
  307. const PinholeGeometry& camera_geometry, CvSize imgSize, double alpha,
  308. CvSize newImgSize) {
  309. cv::Rect_<float> inner, outer;
  310. newImgSize = newImgSize.width * newImgSize.height != 0 ? newImgSize : imgSize;
  311.  
  312. // Get inscribed and circumscribed rectangles in normalized
  313. // (independent of camera matrix) coordinates
  314. icvGetRectangles(camera_geometry, imgSize, inner, outer);
  315.  
  316. // Projection mapping inner rectangle to viewport
  317. double fx0 = (newImgSize.width - 1) / inner.width;
  318. double fy0 = (newImgSize.height - 1) / inner.height;
  319. double cx0 = -fx0 * inner.x;
  320. double cy0 = -fy0 * inner.y;
  321.  
  322. // Projection mapping outer rectangle to viewport
  323. double fx1 = (newImgSize.width - 1) / outer.width;
  324. double fy1 = (newImgSize.height - 1) / outer.height;
  325. double cx1 = -fx1 * outer.x;
  326. double cy1 = -fy1 * outer.y;
  327.  
  328. // Interpolate between the two optimal projections
  329. Eigen::Matrix3d newCameraMatrixEigen = Eigen::Matrix3d::Zero();
  330. newCameraMatrixEigen(0, 0) = fx0 * (1 - alpha) + fx1 * alpha;
  331. newCameraMatrixEigen(1, 1) = fy0 * (1 - alpha) + fy1 * alpha;
  332. newCameraMatrixEigen(0, 2) = cx0 * (1 - alpha) + cx1 * alpha;
  333. newCameraMatrixEigen(1, 2) = cy0 * (1 - alpha) + cy1 * alpha;
  334. newCameraMatrixEigen(2, 2) = 1.0;
  335.  
  336. return newCameraMatrixEigen;
  337. }
  338.  
  339. ///
  340. //Returns the new camera matrix based on the free scaling parameter.
  341. //
  342. // INPUT:
  343. // camera_geometry: camera geometry (distortion and intrinsics used)
  344. // R: Optional rectification transformation in the object space (3x3 matrix)
  345. // newCameraMatrix: New camera matrix
  346. // size: Undistorted image size.
  347. // m1type: Type of the first output map that can be CV_32FC1 or CV_16SC2
  348. //
  349. // OUTPUT: (maps can be used with cv::remap)
  350. // _map1: The first output map.
  351. // _map2: The second output map.
  352. ///
  353. void initUndistortRectifyMap(const PinholeGeometry& _distortedCamera,
  354. const Eigen::Matrix3d& _R,
  355. const Eigen::Matrix3d& _newCameraMatrix, Size size,
  356. int m1type, OutputArray _map1, OutputArray _map2) {
  357.  
  358. //prepare the outputs data structures
  359. if (m1type <= 0)
  360. m1type = CV_16SC2;
  361. CV_Assert(m1type == CV_16SC2 || m1type == CV_32FC1 || m1type == CV_32FC2);
  362. _map1.create(size, m1type);
  363. Mat map1 = _map1.getMat(), map2;
  364. if (m1type != CV_32FC2) {
  365. _map2.create(size, m1type == CV_16SC2 ? CV_16UC1 : CV_32FC1);
  366. map2 = _map2.getMat();
  367. } else
  368. _map2.release();
  369.  
  370. //invert
  371. Eigen::Matrix3d invR = (_newCameraMatrix * _R).inverse();
  372.  
  373. for (int i = 0; i < size.height; i++) {
  374. float* m1f = (float*) (map1.data + map1.step * i);
  375. float* m2f = (float*) (map2.data + map2.step * i);
  376. short* m1 = (short*) m1f;
  377. ushort* m2 = (ushort*) m2f;
  378.  
  379. double _x = i * invR(0, 1) + invR(0, 2), //TODO maybe the is ordering wrong...
  380. _y = i * invR(1, 1) + invR(1, 2), _w = i * invR(2, 1) + invR(2, 2);
  381.  
  382. for (int j = 0; j < size.width;
  383. j++, _x += invR(0, 0), _y += invR(1, 0), _w += invR(2, 0)) {
  384. double w = 1. / _w, x = _x * w, y = _y * w;
  385.  
  386. //apply original distortion
  387. Eigen::Vector2d point(x, y);
  388. _distortedCamera.distortion->distort(point);
  389. double u_norm = point[0], v_norm = point[1];
  390.  
  391. //apply original camera matrix
  392. //get the camera matrix from the camera
  393. Eigen::Matrix3d cameraMatrix = _distortedCamera.getCameraMatrix();
  394.  
  395. double u0 = cameraMatrix(0, 2), v0 = cameraMatrix(1, 2);
  396. double fx = cameraMatrix(0, 0), fy = cameraMatrix(1, 1);
  397.  
  398. double u = fx * u_norm + u0;
  399. double v = fy * v_norm + v0;
  400.  
  401. //store in output format
  402. if (m1type == CV_16SC2) {
  403. int iu = saturate_cast<int>(u * INTER_TAB_SIZE);
  404. int iv = saturate_cast<int>(v * INTER_TAB_SIZE);
  405. m1[j * 2] = (short) (iu >> INTER_BITS);
  406. m1[j * 2 + 1] = (short) (iv >> INTER_BITS);
  407. m2[j] = (ushort) ((iv & (INTER_TAB_SIZE - 1)) * INTER_TAB_SIZE
  408. + (iu & (INTER_TAB_SIZE - 1)));
  409. } else if (m1type == CV_32FC1) {
  410. m1f[j] = (float) u;
  411. m2f[j] = (float) v;
  412. } else {
  413. m1f[j * 2] = (float) u;
  414. m1f[j * 2 + 1] = (float) v;
  415. }
  416. }
  417. }
  418. }
  419.  
  420. }
  421.  
  422. //////////////////////////////////////////////////////
  423. // Undistorter
  424. //////////////////////////////////////////////////////
  425.  
  426. class PinholeUndistorter
  427. {
  428. public:
  429. EIGEN_MAKE_ALIGNED_OPERATOR_NEW
  430.  
  431. /// \param[in] alpha The free scaling parameter between 0 (when all the pixels in the undistorted image will be valid) and 1 (when all the source image pixels will be retained in the undistorted image).
  432. /// \param[in] scale Allows to scale the resulting image.
  433. PinholeUndistorter(const PinholeGeometry& distortedGeometry, double alpha,
  434. double scale, int interpolation)
  435. : distortedGeometry_(distortedGeometry),
  436. interpolation_(interpolation)
  437. {
  438. int width = distortedGeometry_.resolution[0];
  439. int height = distortedGeometry_.resolution[1];
  440. int newWidth = (int) width * scale;
  441. int newHeight = (int) height * scale;
  442.  
  443. // compute the optimal new camera matrix based on the free scaling parameter
  444. Eigen::Matrix3d idealCameraMatrix = Eigen::Matrix3d::Zero();
  445. idealCameraMatrix = cv_helper::getOptimalNewCameraMatrix(distortedGeometry_, cv::Size(width, height), alpha, cv::Size(newWidth, newHeight));
  446.  
  447. // set new idealGeometry
  448. idealGeometry_ = PinholeGeometry(idealCameraMatrix,
  449. Eigen::Vector2i(newWidth, newHeight),
  450. boost::make_shared<NullDistortion>());
  451.  
  452. // compute the undistortion and rectification transformation map
  453. cv_helper::initUndistortRectifyMap(distortedGeometry_,
  454. Eigen::Matrix3d::Identity(),
  455. idealCameraMatrix,
  456. cv::Size(newWidth, newHeight), CV_16SC2,
  457. mapX_, mapY_);
  458. }
  459.  
  460. ~PinholeUndistorter() {};
  461.  
  462. /// \brief Undistort a single image.
  463. void undistortImage(const cv::Mat& inImage, cv::Mat& outImage) const
  464. {
  465. cv::remap(inImage, outImage, mapX_, mapY_, interpolation_);
  466. }
  467.  
  468. private:
  469. const PinholeGeometry distortedGeometry_;
  470. PinholeGeometry idealGeometry_;
  471.  
  472. //undistorter map
  473. cv::Mat mapX_, mapY_;
  474.  
  475. //interpolation type
  476. int interpolation_;
  477. };
  478.  
  479. } //namespace undistorter
  480.  
  481. #endif /* UNDISTORTER_HPP_ */
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!