diff --git a/.gitignore b/.gitignore index 113294a..52e7d8c 100644 --- a/.gitignore +++ b/.gitignore @@ -97,3 +97,5 @@ ENV/ # mypy .mypy_cache/ + +.idea/ diff --git a/README.md b/README.md index 854bc6c..2614643 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,10 @@ # Tsai Calibration +This is a python 3 implementation for stereo cameras calibration and distortion correction by Tsai with distortion method. It is forked from [bailus/tsai-calibration](https://github.com/busyyang/tsai-calibration), which is implemented by python 2. + --- by Busy Yang + +Here is the **original readme** from [bailus/tsai-calibration](https://github.com/busyyang/tsai-calibration): + [Samuel Bailey](http://bailey.geek.nz) <[sam@bailey.geek.nz](mailto:sam@bailey.geek.nz)> @@ -10,7 +15,7 @@ In this implementation a [pinhole camera model](https://en.wikipedia.org/wiki/Pi ## Example -![Left Camera](https://raw.githubusercontent.com/bailus/tsai-calibration/master/output/output-0.png) ![Right Camera](https://raw.githubusercontent.com/bailus/tsai-calibration/master/output/output-1.png) +![Left Camera](output/output-0.png) ![Right Camera](output/output-1.png) | | Left | Right | Stereo | Reference | |---------------------------------------|---------|---------|----------|-----------| @@ -33,10 +38,13 @@ The resulting algorithm is much faster than the traditional methods used in phot ## Requires - * [Python 2.x](https://www.python.org/) + + * Python 3.5+ * [NumPy](http://www.numpy.org/) (Math library) * [Matplotlib](https://matplotlib.org/) (2D/3D plotting library) +For python 2, please visit [bailus/tsai-calibration](https://github.com/busyyang/tsai-calibration). + ## Command Line ~~~~ diff --git a/Tsai_qa.txt b/Tsai_qa.txt new file mode 100644 index 0000000..3c9b234 --- /dev/null +++ b/Tsai_qa.txt @@ -0,0 +1,259 @@ +This is from https://github.com/simonwan1980/Tsai-Camera-Calibration/edit/master/Tsai/faq.txt + +########################################################### +Tsai Camera Calibration FAQ - Reg Willson +Date: Oct 27, 1995 + +If you have any questions, comments, or suggestions about this document, +the software, or camera calibration in general, please read item 5. + +--------------------------- + Table of contents +--------------------------- +1 - What is Tsai's camera model? +2 - Where can I find the required intrinsic camera constants? + 2.1 - Why is the calibrated focal length not the same as the + number printed on the lens? +3 - How do I collect calibration data? + 3.1 - Coplanar and non-coplanar data + 3.2 - Distribution of data points + 3.3 - Placement of world coordinate origin + 3.4 - Perspective projection effects (or lack thereof) + 3.5 - Subpixel measurement of image features +4 - What are some of the nonlinear optimization issues? +5 - Where can I find more information on Tsai's algorithm? +6 - Comments and thanks + +--------------------------- + Contents +--------------------------- + +1 - What is Tsai's camera model? + +Tsai's camera model is based on the pin-hole model of 3D-2D perspective +projection with 1st order radial lens distortion. The model has 11 +parameters: five internal (also called intrinsic or interior) parameters: + + f - effective focal length of the pin-hole camera, + kappa1 - 1st order radial lens distortion coefficient, + Cx, Cy - coordinates of center of radial lens distortion -and- + the piercing point of the camera coordinate frame's + Z axis with the camera's sensor plane, + sx - scale factor to account for any uncertainty in the + framegrabber's resampling of the horizontal scanline. + +and six external (also called extrinsic or exterior) parameters: + + Rx, Ry, Rz - rotation angles for the transform between the + world and camera coordinate frames, + Tx, Ty, Tz - translational components for the transform between the + world and camera coordinate frames. + +In addition to the 11 variable camera parameters Tsai's model has six fixed +intrinsic camera constants: + + Ncx - number of sensor elements in camera's x direction (in sels), + Nfx - number of pixels in frame grabber's x direction (in pixels), + dx - X dimension of camera's sensor element (in mm/sel), + dy - Y dimension of camera's sensor element (in mm/sel), + dpx - effective X dimension of pixel in frame grabber (in mm/pixel), and + dpy - effective Y dimension of pixel in frame grabber (in mm/pixel). + + +2 - Where can I find the required intrinsic camera constants? + +Commercial and industrial cameras often list Ncx, dx, and dy in their +user guides. With consumer cameras you will generally need to contact +the manufacturer. + +You don't need to know the actual values of the intrinsic constants to +calibrate an "accurate" camera model. For most applications "accurate" +simply means that given the 3D world coordinates of a point P the camera +model will accurately predict the 2D position of the point's image P' in +the frame buffer. In these cases the true intrinsic constants aren't +necessary. To get the camera calibration to converge to a solution you +only need the aspect ratio of the camera/frame grabber set up, i.e. the +ratio between dpx and dpy. You can get a good estimate for this from a +straight-on image of a rectangular target. Measuring the rectangle, the +ratio + + (pixels from side-to-side in image) / (mm from side-to-side on target) + ------------------------------------------------------------------------ + (pixels from top-to-bottom in image) / (mm from top-to-bottom on target) + +should be an accurate enough estimate of dpx / dpy to allow the +calibration to converge. The calibration algorithms (with the exception +of the basic Tsai coplanar calibration) will adjust the sx parameter to +automatically compensate for any error in the ratio of dpx / dpy. + +Given the ratio of dpx to dpy, simply pick some value for dpy, say 10um (or +if you know it you can use the actual vertical pixel pitch) and use that to +back calculate dpx, dx, and dy. Set Ncx = Nfx, sx = 1.0, and Cx and Cy to +be the center of the frame buffer. When you calibrate the model the +algorithm will adjust sx, Cx, and Cy to give a best fit set of intrinsic +parameters. + +Observe if you double dpx and dpy, all the extrinsic parameters, Cx, Cy, +and sx, and the calibration error should remain the same. The calibrated +camera model will be just as accurate. The only thing that will change is +f and k1. + + +2.1 - Why is the calibrated focal length not the same as the number + printed on the lens? + +Even if you use the exact intrinsic camera parameters specified by the +manufacturer, the calibrated value of the effective focal length f is +unlikely to be the same as the focal length specified on the lens. In +these calibration algorithms the effective focal length is a parameter +in a pin-hole camera model. The focal length printed on the lens is a +parameter in a thick-lens camera model. While the two parameters have +similar effects on the image they are actually quite different +quantities. + + +3 - How do I collect calibration data? + +Calibration for the model consists of the 3D (x,y,z) world coordinates of a +feature point (in mm) and the corresponding coordinates (Xf,Yf) (in pixels) +of the feature point in the image. The 3D coordinates must be specified in +a right-handed coordinate system. + +Once a camera has been calibrated, subsequent calibrations at different +poses (i.e. with the camera rotated and/or translated) can be speeded up by +using the intrinsic parameters from the first calibration as a starting +point. + + +3.1 - Coplanar and non-coplanar data + +Tsai's algorithm has two variants: one for coplanar data and one for +non-coplanar data. For coplanar data Tsai's algorithm requires the z +component of the 3D coordinates to be 0. + +Basic coplanar calibration requires at least five data points. Basic +non-coplanar calibration requires at least seven data points. Fully +optimized calibration requires at least 11 data points for either +coplanar and non-coplanar data. + +The sx camera parameter cannot be calibrated for using coplanar data. Its +value is left unchanged in the coplanar calibration routines. + + +3.2 - Distribution of data points + +To accurately estimate the radial lens distortion and image center +parameters the calibration data should be distributed broadly across the +field of view. The distribution of data points should, if possible, span +the range of depths that you expect to use the model over. + + +3.3 - Placement of world coordinate system origin + +Tsai's algorithm fails if the origin of the world coordinate system is +near the center of the camera's field of view or near the Y axis of the +camera coordinate system. The Y axis requirement ensures Ty is not +exactly zero which is an explicit requirement in Tsai's algorithm. + +If your calibration data doesn't meet the above criteria you can simply +create a new, artificial coordinate frame for the data that is offset +from the world coordinate frame that you plan on working with. Just add +the offset into the data before you calibrate with it. + + +3.4 - Perspective projection effects (or lack thereof) + +To be able to separate the effects of f and Tz on the image there needs +to be perspective distortion (foreshortening) effects in the calibration +data. For useable perspective distortion the distance between the +calibration points nearest and farthest from the camera should be on the +same scale as the distance between the calibration points and the +camera. This applies both to coplanar and non-coplanar calibration. + +For co-planar calibration the worst case situation is to have the 3D +points lie in a plane parallel to the camera's image plane (all points +an equal distance away). Simple geometry tells us we can't separate the +effects of f and Tz. A relative angle of 30 degrees or more is +recommended to give some effective depth to the data points. + +For non-coplanar calibration the worse case situation is to have the 3D +points lie in a volume of space that is relatively small compared to the +volume's distance to the camera. From a distance the image formation +process is closer to orthographic (not perspective) projection and the +calibration problem becomes poorly conditioned. + + +3.5 - Subpixel measurement of image features + +To obtain accurate calibration data we need to measure the location of +features in the image plane to subpixel accuracy. Three good starting +points on the subject are: + + "Accuracy in Image Measure", Pascal Brand and Roger Mohr, LIFIA - INRIA + Rhones-Alpes, SPIE Vol. 2350 Videometrics III (1994) pages 218-228, + + "An evaluation of subpixel feature localisation methods for precision + measurement", Robert J. Valkenburg, Alan M. McIvor, and P. Wayne Power, + Machine Vision Group, Industrial Research Limited, SPIE Vol. 2350 + Videometrics III (1994) pages 229-238, + +and + + "A comparison of some techniques for the subpixel location of discrete + target images", M. R. Shortis, T. A. Clarke, and T. Short, Department of + Surveying and Land Information, University of Melbourne, SPIE Vol. 2350 + Videometrics III (1994) pages 239-250. + + +4 - What are some of the nonlinear optimization issues? + +Nonlinear optimization for these camera calibration routines is +performed by the MINPACK "lmdif" subroutine. "lmdif" uses a modified +Levenberg-Marquardt algorithm with a jacobian calculated by a +forward-difference approximation. The MINPACK FORTRAN source was +translated into C using the f2c program. + +The function "dpmpar" provides double precision machine parameters for +the MINPACK functions. The routines in this release are set up to use +the machine constants for the IEEE double precision floating point. If +your machine uses a different floating point standard try looking in the +"dpmpar" source to see if the appropriate machine constants are +available. + + +5 - Where can I find more information on Tsai's algorithm? + +An explanation of the basic algorithms and description of the variables +can be found in several publications, including: + + "An Efficient and Accurate Camera Calibration Technique for 3D Machine + Vision", Roger Y. Tsai, Proceedings of IEEE Conference on Computer Vision + and Pattern Recognition, Miami Beach, FL, 1986, pages 364-374. + +and + + "A versatile Camera Calibration Technique for High-Accuracy 3D Machine + Vision Metrology Using Off-the-Shelf TV Cameras and Lenses", Roger Y. Tsai, + IEEE Journal of Robotics and Automation, Vol. RA-3, No. 4, August 1987, + pages 323-344. + + +6 - Comments and thanks + +Questions, comments, suggestions, or bug reports on Tsai's algorithm, this +implementation, or camera calibration in general can be mailed to: +rgwillson@mmm.com. Special thanks to the following persons for +contributing their time, effort, and code! + + o Piotr Jasiobedzki + o Jim Vaughan + o Franz-Josef L|cke + o Pete Rander + o Markus Menke + o Ron Steriti + o Torfi Thorhallsson + o Frederic Devernay + o Volker Rodehorst + o Jon Owen + +########################################################### diff --git a/calibration.py b/calibration.py index 2e77266..68f36f0 100644 --- a/calibration.py +++ b/calibration.py @@ -2,7 +2,8 @@ ### # # Distortion-free Tsai Calibration. -# Given world (3D position on the cube, in mm) and pixel (2D position in the image, in px) cooordinates as input, along with the pixel size and resolution. +# Given world (3D position on the cube, in mm) and pixel (2D position in the image, in px) cooordinates as input, +# along with the pixel size and resolution. # Extracts a set of parameters that describe the camera - see calibrate(points) # Author: Samuel Bailey # @@ -12,14 +13,16 @@ import math from mmath import * - import numpy as np -from numpy.linalg import pinv # Calculate the generalized inverse of a matrix using its singular-value decomposition (SVD) and including all large singular values. -#from scipy.linalg import pinv # Calculate a generalized inverse of a matrix using a least-squares solver. +# Calculate the generalized inverse of a matrix using its singular-value decomposition (SVD) +# and including all large singular values. +from numpy.linalg import pinv +# from scipy.linalg import pinv # Calculate a generalized inverse of a matrix using a least-squares solver. -verbose = False -printVerbose = print if verbose else lambda *a, **k: None # http://stackoverflow.com/questions/5980042/how-to-implement-the-verbose-or-v-option-into-a-script +# http://stackoverflow.com/questions/5980042/how-to-implement-the-verbose-or-v-option-into-a-script +verbose = True +printVerbose = print if verbose else lambda *a, **k: None def approximateL(points): @@ -32,11 +35,12 @@ def constructLineOfM(point): w = point.world xd, yd = d[0], d[1] xw, yw, zw = w[0], w[1], w[2] - return [ yd*xw, yd*yw, yd*zw, yd, -xd*xw, -xd*yw, -xd*zw ] - return np.array(map(constructLineOfM, points), np.float64) + return [yd * xw, yd * yw, yd * zw, yd, -xd * xw, -xd * yw, -xd * zw] + + return np.array([constructLineOfM(p) for p in points]) def constructXd(points): - return np.array(map(lambda point: point.sensor[0], points), np.float64) + return np.array([p.sensor[0] for p in points]) M = constructM(points) inverseM = np.linalg.pinv(M) # M* approximates M inverse @@ -45,50 +49,50 @@ def constructXd(points): def calculateTy(points, L): - #find the point that's furthest from the center of the image + # find the point that's furthest from the center of the image furthestPoint = max(points, key=lambda point: euclideanDistance2d(point.sensor)) - ty = 1.0 / math.sqrt( (L[4]*L[4]) + (L[5]*L[5]) + (L[6]*L[6]) ) - #the above calculation gives the absolute value of Ty + ty = 1.0 / math.sqrt((L[4] * L[4]) + (L[5] * L[5]) + (L[6] * L[6])) + # the above calculation gives the absolute value of Ty - #to find the correct sign we assume ty is positive... - r1 = np.array([ L[0], L[1], L[2] ], np.float64) * ty - r2 = np.array([ L[4], L[5], L[6] ], np.float64) * ty + # to find the correct sign we assume ty is positive... + r1 = np.array([L[0], L[1], L[2]], np.float64) * ty + r2 = np.array([L[4], L[5], L[6]], np.float64) * ty tx = L[3] * ty xt = np.dot(r1, furthestPoint.world) + tx yt = np.dot(r2, furthestPoint.world) + ty - #check our assumption - if (matchingSigns(xt, furthestPoint.sensor[0]) and matchingSigns(yt, furthestPoint.sensor[1])): + # check our assumption + if matchingSigns(xt, furthestPoint.sensor[0] and matchingSigns(yt, furthestPoint.sensor[1])): return ty else: return -ty def calculateSx(L, ty): - return abs(ty) * math.sqrt( (L[0]*L[0]) + (L[1]*L[1]) + (L[2]*L[2]) ) + return abs(ty) * math.sqrt((L[0] * L[0]) + (L[1] * L[1]) + (L[2] * L[2])) def calculateRotation(L, ty, sx): # The first two rows can be calculated from L, ty and sx - r1 = np.array([ L[0], L[1], L[2] ], np.float64) * (ty / sx) - r2 = np.array([ L[4], L[5], L[6] ], np.float64) * ty + r1 = np.array([L[0], L[1], L[2]], np.float64) * (ty / sx) + r2 = np.array([L[4], L[5], L[6]], np.float64) * ty # because r is orthonormal, row 3 can be calculated from row 1 and row 2 r3 = np.cross(r1, r2) - return np.hstack((r1, r2, r3)).reshape(3,3) + return np.hstack((r1, r2, r3)).reshape(3, 3) def calculateTx(L, ty, sx): return L[3] * (ty / sx) -def approximateFTz(points, ty, rotationMatrix): # returns [ f, tz ] +def approximateFTz(points, ty, rotationMatrix): # returns [ f, tz ] ## Construct system of linear equations x = MP where P = (f,tz) - uy = np.add(map(lambda point: np.dot(rotationMatrix[1], point.world), points), ty) - uz = np.array(map(lambda point: np.dot(rotationMatrix[2], point.world), points), np.float64) - yd = np.array(map(lambda point: point.sensor[1], points), np.float64) - M = np.column_stack((uy, map(lambda y: -y, yd))) + uy = np.array([np.dot(rotationMatrix[1], p.world) + ty for p in points]) + uz = np.array([np.dot(rotationMatrix[2], point.world) for point in points]) + yd = np.array([point.sensor[1] for point in points]) + M = np.column_stack((uy, [-y for y in yd])) ## Find an approximation of P inverseM = pinv(M) @@ -98,7 +102,6 @@ def approximateFTz(points, ty, rotationMatrix): # returns [ f, tz ] # returns a set of parameters that describe the camera. assumes a distortion-free pinhole model. def calibrate(points): - L = approximateL(points) ty = calculateTy(points, L) @@ -113,8 +116,8 @@ def calibrate(points): tx = calculateTx(L, ty, sx) printVerbose("tx = %f" % tx) - [ f, tz ] = approximateFTz(points, ty, rotationMatrix) + [f, tz] = approximateFTz(points, ty, rotationMatrix) printVerbose("f = %f" % f) printVerbose("tz = %f" % tz) - return { 'f': f, 'tx': tx, 'ty': ty, 'tz': tz, 'rotationMatrix': rotationMatrix } + return {'f': f, 'tx': tx, 'ty': ty, 'tz': tz, 'rotationMatrix': rotationMatrix} diff --git a/data/3d/shoot2/carm_7th_s7st1_2-1.jpg b/data/3d/shoot2/carm_7th_s7st1_2-1.jpg new file mode 100644 index 0000000..798417b Binary files /dev/null and b/data/3d/shoot2/carm_7th_s7st1_2-1.jpg differ diff --git a/data/3d/shoot2/config.json b/data/3d/shoot2/config.json new file mode 100644 index 0000000..827d3f5 --- /dev/null +++ b/data/3d/shoot2/config.json @@ -0,0 +1,8 @@ + +{ +"images": [ "carm_7th_s7st1_2-1.jpg", "carm_7th_s7st1_2-1.jpg" ], +"resolution": [ 1024, 1024 ], +"pixelSize": [ 0.209, 0.209 ], +"points": "points.csv", +"label": "Siemens Carm" +} \ No newline at end of file diff --git a/data/3d/shoot2/output.pdf b/data/3d/shoot2/output.pdf new file mode 100644 index 0000000..3c0163c Binary files /dev/null and b/data/3d/shoot2/output.pdf differ diff --git a/data/3d/shoot2/points.csv b/data/3d/shoot2/points.csv new file mode 100644 index 0000000..3dd89f9 --- /dev/null +++ b/data/3d/shoot2/points.csv @@ -0,0 +1,76 @@ +-40,80,0,337.91,908.207,337.91,908.207 +-20,80,0,435.268,904.889,435.268,904.889 +0,80,0,531.481,902.508,531.481,902.508 +20,80,0,627.241,901.171,627.241,901.171 +40,80,0,723.744,901.221,723.744,901.221 +-60,60,0,240.513,812.887,240.513,812.887 +-40,60,0,338.422,809.272,338.422,809.272 +-20,60,0,434.596,806.478,434.596,806.478 +0,60,0,530.371,804.478,530.371,804.478 +20,60,0,625.222,802.658,625.222,802.658 +40,60,0,720.838,801.58,720.838,801.58 +60,60,0,817.875,802.155,817.875,802.155 +-80,40,0,141.002,718.602,141.002,718.602 +-60,40,0,240.72,715.392,240.72,715.392 +-40,40,0,337.626,712.611,337.626,712.611 +-20,40,0,433.42,710.435,433.42,710.435 +0,40,0,528.545,708.565,528.545,708.565 +20,40,0,623.506,706.625,623.506,706.625 +40,40,0,718.478,705.404,718.478,705.404 +60,40,0,814.734,704.614,814.734,704.614 +80,40,0,913.915,704.766,913.915,704.766 +-80,20,0,140.523,621.406,140.523,621.406 +-60,20,0,239.75,619.4,239.75,619.4 +-40,20,0,336.465,617.343,336.465,617.343 +-20,20,0,432.077,615.402,432.077,615.402 +0,20,0,527.343,613.465,527.343,613.465 +20,20,0,621.886,611.615,621.886,611.615 +40,20,0,716.539,609.582,716.539,609.582 +60,20,0,812.508,608.519,812.508,608.519 +80,20,0,910.8,607.572,910.8,607.572 +-80,0,0,138.899,524.763,138.899,524.763 +-60,0,0,238.294,523.598,238.294,523.598 +-40,0,0,334.58,522.294,334.58,522.294 +-20,0,0,430.445,520.441,430.445,520.441 +0,0,0,525.508,518.481,525.508,518.481 +20,0,0,620.371,516.522,620.371,516.522 +40,0,0,714.675,514.65,714.675,514.65 +60,0,0,810.615,512.886,810.615,512.886 +80,0,0,908.89,511.37,908.89,511.37 +-80,-20,0,135.966,427.98,135.966,427.98 +-60,-20,0,235.596,427.603,235.596,427.603 +-40,-20,0,332.569,426.69,332.569,426.69 +-20,-20,0,428.17,425.192,428.17,425.192 +0,-20,0,523.373,423.314,523.373,423.314 +20,-20,0,618.101,421.469,618.101,421.469 +40,-20,0,712.868,419.494,712.868,419.494 +60,-20,0,809.31,417.431,809.31,417.431 +80,-20,0,907.998,414.602,907.998,414.602 +-80,-40,0,131.902,330.09,131.902,330.09 +-60,-40,0,232.386,330.866,232.386,330.866 +-40,-40,0,329.547,330.458,329.547,330.458 +-20,-40,0,425.58,329.435,425.58,329.435 +0,-40,0,521.03,327.665,521.03,327.665 +20,-40,0,616.196,325.891,616.196,325.891 +40,-40,0,711.602,323.609,711.602,323.609 +60,-40,0,808.405,320.903,808.405,320.903 +80,-40,0,908.069,317.009,908.069,317.009 +-60,-60,0,227.931,232.157,227.931,232.157 +-40,-60,0,326.596,232.603,326.596,232.603 +-20,-60,0,422.916,232.314,422.916,232.314 +0,-60,0,518.711,230.892,518.711,230.892 +20,-60,0,614.494,228.625,614.494,228.625 +40,-60,0,710.518,226.063,710.518,226.063 +60,-60,0,808.843,222.512,808.843,222.512 +-40,-80,0,322.308,131.921,322.308,131.921 +-20,-80,0,419.904,131.955,419.904,131.955 +0,-80,0,516.548,131.034,516.548,131.034 +20,-80,0,613.206,129.013,613.206,129.013 +40,-80,0,710.883,125.68,710.883,125.68 +30,10,0,668.246,563.193,668.246,563.193 +27.75,-9.25,-72,666.399,467.182,666.399,467.182 +27.75,-27.75,-72,664.677,372.082,664.677,372.082 +-9.25,-27.75,-72,475.012,375.787,475.012,375.787 +-30,-10,0,381.554,473.545,381.554,473.545 +-10,30,0,480.408,661.854,480.408,661.854 +9.25,27.75,-72,575.386,658.812,575.386,658.812 \ No newline at end of file diff --git a/distortion.py b/distortion.py index 97339f8..5c0ed2d 100644 --- a/distortion.py +++ b/distortion.py @@ -2,7 +2,8 @@ ### # # 2-stage Tsai Calibration with Distortion. -# Given world (3D position on the cube, in mm) and pixel (2D position in the image, in px) cooordinates as input, along with the pixel size and resolution. +# Given world (3D position on the cube, in mm) and pixel (2D position in the image, in px) cooordinates as input, +# along with the pixel size and resolution. # Extracts a set of parameters that describe the camera - see calibrateDistorted(points) # Author: Samuel Bailey # @@ -10,28 +11,52 @@ from __future__ import print_function + +import math + import numpy as np -import scipy as sp + from transforms import * from calibration import calibrate - -verbose = False -printVerbose = print if verbose else lambda *a, **k: None # http://stackoverflow.com/questions/5980042/how-to-implement-the-verbose-or-v-option-into-a-script +# http://stackoverflow.com/questions/5980042/how-to-implement-the-verbose-or-v-option-into-a-script +verbose = True +printVerbose = print if verbose else lambda *a, **k: None def error(points): - numbers = map(lambda p: np.linalg.norm(np.subtract(p.pixel[:2], p.distortedPixel[:2])), points) - return { 'mean': np.mean(numbers), 'median': np.median(numbers), 'min': np.amin(numbers), 'max': np.amax(numbers) } + numbers = [np.linalg.norm(np.subtract(p.pixel[:2], p.distortedPixel[:2])) for p in points] + return {'mean': np.mean(numbers), 'median': np.median(numbers), 'min': np.amin(numbers), 'max': np.amax(numbers)} def estimateKappa(points): def estimateKappaP(point): - u2 = (point.projectedSensor[0]*point.projectedSensor[0]) + (point.projectedSensor[1]*point.projectedSensor[1]) - d2 = (point.sensor[0]*point.sensor[0]) + (point.sensor[1]*point.sensor[1]) + """ + x_u = x_d(1+k*r^2) + y_u = y_d(1+k*r^2) where r^2 = (x_d)^2 + (y_d)^2 + + (x_u)^2 + (y_u)^2 = [(x_d)^2 + (y_d)^2] [(1+k*r^2)^2] + note u2 = (x_u)^2 + (y_u)^2, and d2 = (x_d)^2 + (y_d)^2 + so, u2 = d2[(1+k*d2)^2] + then, k = (sqrt(u / d) - 1) / d2 + + if u2>d2, the value of k should be positive, otherwise it should be nagitive. + + :param point: + :return: + """ + + u2 = (point.projectedSensor[0] * point.projectedSensor[0]) + ( + point.projectedSensor[1] * point.projectedSensor[1]) + d2 = (point.sensor[0] * point.sensor[0]) + (point.sensor[1] * point.sensor[1]) + d = math.sqrt(d2) - return (u2-d2)/(d2*d) - return -np.mean(map(estimateKappaP, points)) + u = math.sqrt(u2) + k = (u / d - 1) / d2 + return k + + return np.mean(list(map(estimateKappaP, points))) + def calibrateDistorted(settings, points, image): pixelSize = settings['pixelSize'] @@ -40,51 +65,85 @@ def calibrateDistorted(settings, points, image): yOffset = settings['yOffset'] numLowDistortionPoints = settings['minLowDistortionPoints'] numHighDistortionPoints = settings['numHighDistortionPoints'] - passes = settings['passes'] - + points = pixelToSensor(points, resolution, pixelSize) # split the data into low/high distortion points points = sorted(points, key=lambda p: euclideanDistance2d(p.sensor)) printVerbose('%d points' % len(points)) - lowDistortionPoints = points[:16] - printVerbose('%d low distortion points, max. distance from center of sensor = %fmm' % (len(lowDistortionPoints), np.max(map(lambda p: np.linalg.norm(p.sensor[:2]), lowDistortionPoints)))) + lowDistortionPoints = points[:numLowDistortionPoints] + printVerbose('%d low distortion points, max. distance from center of sensor = %fmm' % ( + len(lowDistortionPoints), np.max(list(map(lambda p: np.linalg.norm(p.sensor[:2]), lowDistortionPoints))))) + highDistortionPoints = points[-numHighDistortionPoints:] - printVerbose('%d high distortion points, min. distance from center of sensor = %fmm' % (len(highDistortionPoints), np.min(map(lambda p: np.linalg.norm(p.sensor[:2]), highDistortionPoints)))) + printVerbose('%d high distortion points, min. distance from center of sensor = %fmm' % ( + len(highDistortionPoints), np.min(list(map(lambda p: np.linalg.norm(p.sensor[:2]), highDistortionPoints))))) - kappa = 0.0 # assume K1 = 0 (no distortion) for the initial calibration + kappa = 0.0 # assume K1 = 0 (no distortion) for the initial calibration # record some basic statistics errors = [] kappas = [] + def stats(): e = error(points) errors.append(e) kappas.append(kappa) + print(e) + print("kappe = ", kappa) return e - #re-calibrate, re-estimate kappa, repeat - for i in range(passes+1): - - #use the estimated K1 to "undistort" the location of the points in the image, - # then calibrate (using the points with low distortion.) - params = calibrate(pixelToSensor(lowDistortionPoints, resolution, pixelSize, kappa)) - - points = worldToPixel(points, params, pixelSize, resolution, yOffset, kappa) - stats() - - #use the new calibration parameters to re-estimate kappa (using the points with high distortion.) - kappa = estimateKappa(worldToPixel(highDistortionPoints, params, pixelSize, resolution, yOffset, kappa)) - - #project the points in world coordinates back onto the sensor then record some stats - points = worldToPixel(points, params, pixelSize, resolution, yOffset, kappa) - stats() - - if passes == 2: - numLowDistortionPoints = maxLowDistortionPoints - lowDistortionPoints = points[:max(numLowDistortionPoints, settings['maxLowDistortionPoints'])] + # step1: use the central fiducials to calibrate with k1=0 + params = calibrate(pixelToSensor(lowDistortionPoints, resolution, pixelSize, kappa=0.0)) + + # step2: optimize f, Tz, k1 by non-linear method + + xc = np.array([point.sensor[0] for point in highDistortionPoints]) + yc = np.array([point.sensor[1] for point in highDistortionPoints]) + R = np.array(params['rotationMatrix']) + Pw = np.array([point.world for point in highDistortionPoints]) + Tx = params['tx'] + Ty = params['ty'] + + ## for init values + x0 = np.array([0, params['f'], params['tz']]) # k1,f,Tz + + def obj(x): + """ + + x_u = x_d(1+k*r^2)=f(r1*Pw+Tx) / (r3*Pw+Tz) + so minimize F = ||x_d(1+k*r^2) * (r3*Pw+Tz) - f(r1*Pw+Tx)||^2 + + xc: value of x-axis for 2d points in camera coordinate (xc = (xi-u0)*dx) + yc: value of y-axis for 2d points in camera coordinate (yc = (yi-v0)*dy) + R: rotation matrix of world to camera coordinate + Pw: 3d points in world coordinate + Tx: translation of x-axis for world to camera coordinate + Ty: translation of y-axis for world to camera coordinate + :param args: + k1: the kappa value for 1st order radial distortion + f: focal length + Tz: translation of z-axis for world to camera coordinate + :return: + """ + k1, f, Tz = x[0], x[1], x[2] + r2 = xc * xc + yc * yc + vx = xc * (1 + k1 * r2) * (np.sum(R[2, :] * Pw, axis=1) + Tz) - f * (np.sum(R[0, :] * Pw, axis=1) + Tx) + vy = yc * (1 + k1 * r2) * (np.sum(R[2, :] * Pw, axis=1) + Tz) - f * (np.sum(R[1, :] * Pw, axis=1) + Ty) + value = np.sum(np.power(vx,2) + np.power(vy,2)) + print(f'value = {value},k1 = {k1}, f = {f}, Tz = {Tz}') + return value + + res = minimize(obj, x0, method='Powell') + print(res.fun, '\n', res.success, '\n', res.x) + kappa = res.x[0] + params['f'] = res.x[1] + params['tz'] = res.x[2] + points = worldToPixel(points, params, pixelSize, resolution, yOffset, kappa) + stats() + + translationVector = np.array([params['tx'], params['ty'], params['tz']], np.float64) - translationVector = np.array([ params['tx'], params['ty'], params['tz'] ], np.float64) return { 'label': label, 'params': { diff --git a/main.py b/main.py index 6068f1e..cf74618 100644 --- a/main.py +++ b/main.py @@ -11,6 +11,7 @@ from __future__ import print_function import json import math +import os.path from pprint import pprint # and some math stuff @@ -18,13 +19,13 @@ # and display some points import matplotlib + matplotlib.use('agg') from matplotlib.backends.backend_pdf import PdfPages -verbose = False -printVerbose = print if verbose else lambda *a, **k: None # http://stackoverflow.com/questions/5980042/how-to-implement-the-verbose-or-v-option-into-a-script - - +# http://stackoverflow.com/questions/5980042/how-to-implement-the-verbose-or-v-option-into-a-script +verbose = True +printVerbose = print if verbose else lambda *a, **k: None from mmath import * from point import Point, newPoint @@ -34,27 +35,25 @@ from plot import * - def processStereo(leftCamera, rightCamera): - worldPoints = map(lambda p: p.world, leftCamera['points']) + worldPoints = [p.world for p in leftCamera['points']] leftParams = leftCamera['params'] rightParams = rightCamera['params'] print('\nCamera: %s' % leftCamera['label']) print('\n Left:') pprint(leftParams) - print('\n Right:') pprint(rightParams) - - baseline = np.linalg.norm(leftParams['translationVector']-rightParams['translationVector']) + + baseline = np.linalg.norm(leftParams['translationVector'] - rightParams['translationVector']) print('\n baseline:') pprint(baseline) print('\n distance to camera:') - camera_midpoint = np.linalg.norm( ( leftParams['translationVector'] + rightParams['translationVector'] ) / 2.0 ) + camera_midpoint = np.linalg.norm((leftParams['translationVector'] + rightParams['translationVector']) / 2.0) print(camera_midpoint) return { @@ -64,57 +63,80 @@ def processStereo(leftCamera, rightCamera): def openFile(settings, folder): - dataFilename = '%s/config.json' % folder + dataFilename = os.path.join(folder, 'config.json') with open(dataFilename) as dataFile: data = json.load(dataFile) - for n in [ 'pixelSize', 'resolution', 'label' ]: + for n in ['pixelSize', 'resolution', 'label']: settings[n] = data[n] - def readCsvLine(csvLine): - values = map(lambda v: float(v), csvLine.split(',')) - return { 'left': newPoint({ 'world': values[:3], 'pixel': values[-4:-2] }), 'right': newPoint({ 'world': values[:3], 'pixel': values[-2:] }) } - - with open('%s/%s' % (folder, data['points'])) as csvFile: - points = map(readCsvLine, csvFile.readlines()) - - leftImage = plt.imread('%s/%s' % (folder, data['images'][0])) - rightImage = plt.imread('%s/%s' % (folder, data['images'][1])) - - leftPoints, rightPoints = map(lambda o: o['left'], points), map(lambda o: o['right'], points) + def readCsvLine(csvLines: str): + points = [] + for csvLine in csvLines: + values = [float(v) for v in csvLine.strip().split(',')] + points.append( + {'left': newPoint({'world': values[:3], 'pixel': values[-4:-2]}), + 'right': newPoint({'world': values[:3], 'pixel': values[-2:]})} + ) + return points + + with open(os.path.join(folder, data['points'])) as csvFile: + points = readCsvLine(csvFile.readlines()) + + os.path.join(folder, data['images'][0]) + leftImage = plt.imread(os.path.join(folder, data['images'][0])) + rightImage = plt.imread(os.path.join(folder, data['images'][1])) + + leftPoints = [o['left'] for o in points] + rightPoints = [o['right'] for o in points] leftCamera = calibrateDistorted(settings, leftPoints, leftImage) rightCamera = calibrateDistorted(settings, rightPoints, rightImage) world = processStereo(leftCamera, rightCamera) - return { 'left': leftCamera, 'right': rightCamera, 'world': world } + return {'left': leftCamera, 'right': rightCamera, 'world': world} def openFolders(settings): - stats = map(lambda folder: openFile(settings, folder), settings['folders']) + stats = [openFile(settings, folder) for folder in settings['folders']] with PdfPages(settings['outputFilename']) as pdf: def p(s, l): pdf.savefig(plotPoints(s['points'], s['image'], l)) + def f(x): p(x['left'], 'Left Sensor') p(x['right'], 'Right Sensor') pdf.savefig(displayStereo(x)) pdf.savefig(displayStereoSide(x)) - map(f, stats) - + + [f(stat) for stat in stats] + def main(): + + settings = { + 'camera': 'Siemens Carm', + 'yOffset': 0.0, + 'minLowDistortionPoints': 16, + 'numHighDistortionPoints': 60, + 'passes': 8, + 'folders': ['data/3d/shoot2'], + 'outputFilename': 'data/3d/shoot2/output.pdf' + } + """ settings = { 'camera': 'GoPro Hero 3+ Stereo', 'yOffset': 0.0, - 'minLowDistortionPoints': 8, - 'maxLowDistortionPoints': 18, + 'minLowDistortionPoints': 16, 'numHighDistortionPoints': 8, 'passes': 8, - 'folders': [ 'data/3d/shoot1' ], + 'folders': ['data/3d/shoot1'], 'outputFilename': 'output/output.pdf' } + """ openFolders(settings) -main() + +if __name__ == '__main__': + main() diff --git a/mmath.py b/mmath.py index 41a7ef4..14ea87e 100644 --- a/mmath.py +++ b/mmath.py @@ -10,21 +10,21 @@ import numpy as np -def euclideanDistance2d(x): #euclidean distance from x to (0,0) - return sqrt((x[0]*x[0]) + (x[1]*x[1])) +def euclideanDistance2d(x): # euclidean distance from x to (0,0) + return sqrt((x[0] * x[0]) + (x[1] * x[1])) -def matchingSigns(x, y): #returns true if x and y are both negative or both non-negative +def matchingSigns(x, y): # returns true if x and y are both negative or both non-negative return (x < 0.0 and y < 0.0) or (x >= 0.0 and y >= 0.0) - -def rotationToHomogeneous(r): #rotation matrix to homogeneous transformation + +def rotationToHomogeneous(r): # rotation matrix to homogeneous transformation q = np.transpose(np.array([r[0], r[1], r[2], np.zeros(3)], np.float64)) - return np.transpose(np.array([ q[0], q[1], q[2], [ 0.0, 0.0, 0.0, 1.0 ] ], np.float64)) + return np.transpose(np.array([q[0], q[1], q[2], [0.0, 0.0, 0.0, 1.0]], np.float64)) -def translationToHomogeneous(t): #translation vector to homogeneous transformation - tt = np.transpose(np.array([ np.zeros(4), np.zeros(4), np.zeros(4), [ t[0], t[1], t[2], 0.0 ] ], np.float64)) +def translationToHomogeneous(t): # translation vector to homogeneous transformation + tt = np.transpose(np.array([np.zeros(4), np.zeros(4), np.zeros(4), [t[0], t[1], t[2], 0.0]], np.float64)) return tt + np.identity(4) @@ -35,7 +35,7 @@ def matrixToEuler(M): return heading, attitude, bank -def eulerToMatrix(( heading, attitude, bank )): +def eulerToMatrix(heading, attitude, bank): # Convert euler angles back to matrix sa, ca = sin(attitude), cos(attitude) sb, cb = sin(bank), cos(bank) @@ -43,27 +43,27 @@ def eulerToMatrix(( heading, attitude, bank )): return [ [ - ch*ca, - (-ch*sa*cb) + (sh*sb), - (ch*sa*sb) + (sh*cb) + ch * ca, + (-ch * sa * cb) + (sh * sb), + (ch * sa * sb) + (sh * cb) ], [ sa, - ca*cb, - -ca*sb + ca * cb, + -ca * sb ], [ - -sh*ca, - (sh*sa*cb) + (ch*sb), - (-sh*sa*sb) + (ch*cb) + -sh * ca, + (sh * sa * cb) + (ch * sb), + (-sh * sa * sb) + (ch * cb) ] ] # Given an almost-orthonormal matrix M, returns the closest orthonormal (ie. rotation) matrix def makeOrthonormal(M): - return eulerToMatrix(matrixToEuler(M)) + return eulerToMatrix(*matrixToEuler(M)) def clamp(value, low=0.0, high=1.0): - return min(max(value, low), high) + return min(max(value, low), high) diff --git a/others/TsaiCpp/Source.cpp.xyz b/others/TsaiCpp/Source.cpp.xyz new file mode 100644 index 0000000..fe7efb4 --- /dev/null +++ b/others/TsaiCpp/Source.cpp.xyz @@ -0,0 +1,142 @@ +#include "TsaiCalibration.h.xyzh" + + + + +void main() +{ + + double image_w = 1024; + double image_h = 1024; + // 内参 + double f = 1011; + double dx = 0.209, dy = 0.209; + double u0 = 505, v0 = 509.1; + + // 外参 + double rx = 0.001; + double ry = -0.001; + double rz = 0.002; + + double tx = 10; + double ty = -10; + double tz = 950; + + // 畸变参数 + double k1 = 5; + double k2 = 100; + double p1 = -0.01; + double p2 = -0.01; + double s1 = -0.02; + double s3 = 0.01; + + cv::Mat camera_matrix = (cv::Mat_(3, 3) << f / dx, 0, u0, 0, f / dy, v0, 0, 0, 1); + cv::Mat camera_matrix_undistortion = (cv::Mat_(3, 3) << f / dx, 0, (image_w - 1.0) / 2, 0, f / dy, (image_h - 1.0) / 2, 0, 0, 1); + cv::Mat rotation = TsaiCalibration::Angle2RotationMat(rx, ry, rz); + cv::Mat rvec = (cv::Mat_(3, 1) << rz, ry, rz); + cv::Mat translation = (cv::Mat_(3, 1) << tx, ty, tz); + + // k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4 + cv::Mat distortion_coeff = (cv::Mat_(12, 1) << k1, k2, p1, p2, 0, 0, 0, 0, s1, 0, s3, 0); + + std::vector points3D = + { +#include "marker3d.data" + }; + + std::vector points2D; + cv::projectPoints(points3D, rvec, translation, camera_matrix, distortion_coeff, points2D); + + cv::Mat distort_image = cv::Mat(image_h, image_w, CV_8UC3,cv::Scalar::all(255)); + for (size_t i = 0; i < points2D.size(); i++) + { + cv::circle(distort_image, cv::Point(points2D[i]), 10, cv::Scalar(0, 0, 255), 2); + } + + for (size_t i = 0; i < 10; i++) + { + cv::line(distort_image, cv::Point(0, i * image_h / 10), cv::Point(image_w, i* image_h / 10), cv::Scalar::all(50)); + cv::line(distort_image, cv::Point(i * image_w / 10, 0), cv::Point(i* image_w / 10, image_h), cv::Scalar::all(50)); + } + cv::imwrite("distort_image.png", distort_image); + + if (1) + { + TsaiCalibration tsai3; + tsai3.SetImageSize(image_w, image_h); + tsai3.Setf(f); + tsai3.SetPoints2D(points2D); + tsai3.SetPoints3D(points3D); + + if (!tsai3.Execute(TsaiCalibration::ThreeStepOptimation)) + { + std::cout << "Errors when Three StepOptimization!" << std::endl; + exit(-1); + } + double e = tsai3.GetProjectionError(); + std::cout << tsai3.GetCalibratedParameters() << std::endl; + + cv::Mat undistored_image = tsai3.UndistortImage(distort_image); + std::vector projected_undistortion_points = tsai3.GetUndistortionProjectedPoints(); + for (size_t i = 0; i < projected_undistortion_points.size(); i++) + { + cv::drawMarker(undistored_image, cv::Point(projected_undistortion_points[i]), cv::Scalar::all(0)); + } + + std::vector undistored_points = tsai3.GetUndistortedPoints(); + for (size_t i = 0; i < undistored_points.size(); i++) + { + cv::drawMarker(undistored_image, cv::Point(undistored_points[i]), cv::Scalar(255, 0, 0), cv::MarkerTypes::MARKER_SQUARE); + } + cv::imwrite("undistort_image_tsai3.png", undistored_image); + } + + + if (1) + { + std::vector inner3D; + std::vector inner2D; + + std::vector innerIndex = { 24,25,26,33,34,35,42,43,44,69,70,71,72,73,74,75 }; + for (const auto i : innerIndex) + { + inner3D.push_back(points3D[i]); + inner2D.push_back(points2D[i]); + } + + + TsaiCalibration tsai5; + tsai5.SetImageSize(image_w, image_h); + tsai5.Setf(f); + tsai5.SetPoints3D(points3D); + tsai5.SetPoints2D(points2D); + + tsai5.SetInnerPoints3D(inner3D); + tsai5.SetInnerPoints2D(inner2D); + + if (!tsai5.Execute(TsaiCalibration::FiveStepOptimization)) + { + std::cout << "Errors when Five StepOptimization!" << std::endl; + exit(-2); + } + double e = tsai5.GetProjectionError(); + std::cout << tsai5.GetCalibratedParameters() << std::endl; + + cv::Mat undistored_image = tsai5.UndistortImage(distort_image); + std::vector projected_undistortion_points = tsai5.GetUndistortionProjectedPoints(); + for (size_t i = 0; i < projected_undistortion_points.size(); i++) + { + cv::drawMarker(undistored_image, cv::Point(projected_undistortion_points[i]), cv::Scalar::all(0)); + } + + std::vector undistored_points = tsai5.GetUndistortedPoints(); + for (size_t i = 0; i < undistored_points.size(); i++) + { + cv::drawMarker(undistored_image, cv::Point(undistored_points[i]), cv::Scalar(255, 0, 0), cv::MarkerTypes::MARKER_SQUARE); + } + cv::imwrite("undistort_image_tsai5.png", undistored_image); + } + + + +} \ No newline at end of file diff --git a/others/TsaiCpp/Source3.cpp.xyz b/others/TsaiCpp/Source3.cpp.xyz new file mode 100644 index 0000000..c6f8f33 --- /dev/null +++ b/others/TsaiCpp/Source3.cpp.xyz @@ -0,0 +1,1178 @@ +#include +#include +#include + +#include "matrixs/matrixs.hpp" +#include "matrixs/optimization.hpp" + + + +static cv::Mat angle2mat(const double& Rx, const double& Ry, const double& Rz) +{ + double sa = sin(Rx); + double ca = cos(Rx); + double sb = sin(Ry); + double cb = cos(Ry); + double sg = sin(Rz); + double cg = cos(Rz); + + cv::Mat rm(3, 3, CV_64FC1); + + rm.at(0, 0) = cb * cg; + rm.at(0, 1) = cg * sa * sb - ca * sg; + rm.at(0, 2) = sa * sg + ca * cg * sb; + rm.at(1, 0) = cb * sg; + rm.at(1, 1) = sa * sb * sg + ca * cg; + rm.at(1, 2) = ca * sb * sg - cg * sa; + rm.at(2, 0) = -sb; + rm.at(2, 1) = cb * sa; + rm.at(2, 2) = ca * cb; + + return rm; +} + +static std::array mat2angle(const cv::Mat& rm) +{ + double r1, r2, r3, r4, r5, r6, r7, sg, cg; + if (rm.empty() || rm.cols != 3 || rm.cols != 3) + { + return {}; + } + r1 = rm.at(0, 0); + r2 = rm.at(0, 1); + r3 = rm.at(0, 2); + r4 = rm.at(1, 0); + r5 = rm.at(1, 1); + r6 = rm.at(1, 2); + r7 = rm.at(2, 0); + + double Rz = atan2(r4, r1); + sg = sin(Rz); + cg = cos(Rz); + double Ry = atan2(-r7, r1 * cg + r4 * sg); + double Rx = atan2(r3 * sg - r6 * cg, r5 * cg - r2 * sg); + + return { Rx,Ry,Rz }; +} + +struct CarmTransformParameters +{ + + cv::Vec3d camera_pose; /* the position of C-arm camera*/ + cv::Mat rotation_matrix; /* the rotation matrix from physical coordinate to camera coordinate. */ + cv::Mat translation_vector; /* the translation vector from physical coordinate to camera coordinate. */ + cv::Vec3d image_origin; /* the origion of xray image in physical coordinate. */ + cv::Vec3d normal_vector_of_plane; /* the normal vector of image plane */ + cv::Vec3d image_spacing; /* the spacing of x-ray image. */ + double k1; /*1st order radial distortion coeffient*/ + double k2; /*2nd order radial distortion coeffient*/ + double p1; /*1st order tangential distortion coeffient*/ + double p2; /*2nd order tangential distortion coeffient*/ + double s1; /*1st order thin prism distortion coeffient for x*/ + double s3; /*1st order thin prism distortion coeffient for y*/ + double f; /* the distance between x-ray source and detector.*/ + double kx; + double ky; + double u0; + double v0; + CarmTransformParameters() : + k1(0), kx(0), ky(0), u0(0), v0(0), f(1011) {}; + + friend std::ostream& operator<<(std::ostream& os, const CarmTransformParameters& para) + { + os << "Carm Transform Parameter: " << std::endl; + os << "\tf = " << para.f << std::endl; + os << "\tkx = " << para.kx << std::endl; + os << "\tky = " << para.ky << std::endl; + //os << "\tCamera Pose = " << para.camera_pose << std::endl; + //os << "\tRotation Matrix = \n" << para.rotation_matrix << std::endl; + os << "\tTranslation Vector = " << para.translation_vector.t() << std::endl; + os << "\tAngles = " << mat2angle(para.rotation_matrix)[0] << "," << mat2angle(para.rotation_matrix)[1] << "," << mat2angle(para.rotation_matrix)[2] << std::endl; + os << "\tImage Spacing = " << para.image_spacing << std::endl; + os << "\tsx = " << para.image_spacing[1] / para.image_spacing[0] << std::endl; + os << "\tu0 = " << para.u0 << std::endl; + os << "\tv0 = " << para.v0 << std::endl; + os << "\tdx = " << para.image_spacing[0] << std::endl; + os << "\tdy = " << para.image_spacing[1] << std::endl; + os << "\tk1 = " << para.k1 << std::endl; + os << "\tk2 = " << para.k2 << std::endl; + os << "\tp1 = " << para.p1 << std::endl; + os << "\tp2 = " << para.p2 << std::endl; + os << "\ts1 = " << para.s1 << std::endl; + os << "\ts3 = " << para.s3 << std::endl; + + + + return os; + } +}; + +static std::vector m_ImageSize({ 1024,1024 }); +static std::vector m_ImageSpacing({ 0.209,0.209 }); +static CarmTransformParameters m_CarmTransformParameters; + + + +void set_data(std::vector& points3D, std::vector& points2D) +{ + points3D.clear(); + points2D.clear(); + //set data + std::vector> data = { +#include "points.data" + }; + for (size_t i = 0; i < data.size(); i++) + { + points3D.push_back(cv::Vec3d(data[i][0], data[i][1], data[i][2])); + points2D.push_back(cv::Vec2d(data[i][3], data[i][4])); + } +} + +void set_inner_data(std::vector& points3D, std::vector& points2D) +{ + points3D.clear(); + points2D.clear(); + //set data + std::vector> data = { +#include "points_inner.data" + }; + for (size_t i = 0; i < data.size(); i++) + { + points3D.push_back(cv::Vec3d(data[i][0], data[i][1], data[i][2])); + points2D.push_back(cv::Vec2d(data[i][3], data[i][4])); + } +} + + +void ProjectErrorStat(std::vector& markers3DPoints, std::vector& image2DPoints) +{ + cv::Mat matIntrinsic = (cv::Mat_(3, 3) << m_CarmTransformParameters.kx, 0, m_CarmTransformParameters.u0, + 0, m_CarmTransformParameters.ky, m_CarmTransformParameters.v0, 0, 0, 1); + std::vector errors; + + cv::Mat rotation_matrix = m_CarmTransformParameters.rotation_matrix; + cv::Mat translation_vector = m_CarmTransformParameters.translation_vector; + for (size_t i = 0; i < image2DPoints.size(); i++) + { + cv::Mat p3d = (cv::Mat_(3, 1) << markers3DPoints[i][0], markers3DPoints[i][1], markers3DPoints[i][2]); + cv::Mat pu = matIntrinsic * (rotation_matrix * p3d + translation_vector); + pu = pu / pu.at(2, 0); // point in pixel unit without distortion + + cv::Mat pd = (cv::Mat_(2, 1) << (image2DPoints[i][0] - m_CarmTransformParameters.u0) * m_CarmTransformParameters.image_spacing[0], + (image2DPoints[i][1] - m_CarmTransformParameters.v0) * m_CarmTransformParameters.image_spacing[1]); // point in camera coordinate with distortion + + double r2 = std::pow(pd.at(0, 0), 2) + std::pow(pd.at(1, 0), 2); + cv::Mat upd = pd * (1 + m_CarmTransformParameters.k1 * r2); //undistored point in camera coordinate + upd = (cv::Mat_(2, 1) << upd.at(0, 0) / m_CarmTransformParameters.image_spacing[0] + m_CarmTransformParameters.u0, + upd.at(1, 0) / m_CarmTransformParameters.image_spacing[1] + m_CarmTransformParameters.v0); // undistored point in pixel unit + + double error = cv::norm(pu.rowRange(0, 2) - upd); + errors.push_back(error); + + } + + double maxe = *std::max_element(errors.begin(), errors.end()); + double mine = *std::min_element(errors.begin(), errors.end()); + double meane = std::reduce(errors.begin(), errors.end()) / errors.size(); + std::cout << "Max Projection Error = " << maxe << std::endl; + std::cout << "Min Projection Error = " << mine << std::endl; + std::cout << "Mean Projection Error = " << meane << std::endl; + +} + + +void ProjectErrorStatOpenCV(std::vector& markers3DPoints, std::vector& image2DPoints) +{ + cv::Mat matIntrinsic = (cv::Mat_(3, 3) << m_CarmTransformParameters.kx, 0, m_CarmTransformParameters.u0, + 0, m_CarmTransformParameters.ky, m_CarmTransformParameters.v0, 0, 0, 1); + std::vector errors; + + cv::Mat rotation_matrix = m_CarmTransformParameters.rotation_matrix; + cv::Mat translation_vector = m_CarmTransformParameters.translation_vector; + for (size_t i = 0; i < image2DPoints.size(); i++) + { + + double k1 = m_CarmTransformParameters.k1; + double k2 = m_CarmTransformParameters.k2; + double p1 = m_CarmTransformParameters.p1; + double p2 = m_CarmTransformParameters.p2; + double s1 = m_CarmTransformParameters.s1; + double s3 = m_CarmTransformParameters.s3; + + cv::Mat p3d = (cv::Mat_(3, 1) << markers3DPoints[i][0], markers3DPoints[i][1], markers3DPoints[i][2]); + cv::Mat pu = rotation_matrix * p3d + translation_vector; + pu = pu / pu.at(2, 0); // point in unit camera coordinate unit without distortion + + + double r2 = std::pow(pu.at(0, 0), 2) + std::pow(pu.at(1, 0), 2); + double xp = pu.at(0, 0); + double yp = pu.at(1, 0); + + double xd = xp * (1 + k1 * r2 + k2 * r2 * r2) + 2 * p1 * xp * yp + p2 * (r2 + 2 * xp * xp) + s1 * r2; + double yd = yp * (1 + k1 * r2 + k2 * r2 * r2) + p1 * (r2 + 2 * yp * yp) + 2 * p2 * xp * yp + s3 * r2; + + cv::Mat pd = (cv::Mat_(2, 1) << xd, yd); + pd.at(0, 0) = pd.at(0, 0) * m_CarmTransformParameters.kx + m_CarmTransformParameters.u0; + pd.at(1, 0) = pd.at(1, 0) * m_CarmTransformParameters.ky + m_CarmTransformParameters.v0; + + double error = std::pow(pd.at(0, 0) - image2DPoints[i][0], 2) + std::pow(pd.at(1, 0) - image2DPoints[i][1], 2); + error = sqrt(error); + errors.push_back(error); + + } + + double maxe = *std::max_element(errors.begin(), errors.end()); + double mine = *std::min_element(errors.begin(), errors.end()); + double meane = std::reduce(errors.begin(), errors.end()) / errors.size(); + std::cout << "Max Projection Error = " << maxe << std::endl; + std::cout << "Min Projection Error = " << mine << std::endl; + std::cout << "Mean Projection Error = " << meane << std::endl; + +} + + + + + +bool Tsai(std::vector& markers3DPoints, std::vector& image2DPoints) +{ + if (markers3DPoints.size() != image2DPoints.size()) + { + return false; + } + int size = markers3DPoints.size(); + // compute r11,r12,r13,r21,r22,r23,Tx,Ty + double offset_x = (m_ImageSize[0] - 1.0) / 2.0; + double offset_y = (m_ImageSize[1] - 1.0) / 2.0; + //double offset_x = m_CarmTransformParameters.u0; + //double offset_y = m_CarmTransformParameters.v0; + + cv::Vec3d spacing = m_CarmTransformParameters.image_spacing; + + cv::Mat A = cv::Mat_(size, 7); + cv::Mat B = cv::Mat_(size, 1); + for (size_t i = 0; i < size; i++) + { + double xi = (image2DPoints[i][0] - offset_x); + double yi = (image2DPoints[i][1] - offset_y); + + A.at(i, 0) = xi * markers3DPoints[i][0]; + A.at(i, 1) = xi * markers3DPoints[i][1]; + A.at(i, 2) = xi * markers3DPoints[i][2]; + A.at(i, 3) = -yi * markers3DPoints[i][0]; + A.at(i, 4) = -yi * markers3DPoints[i][1]; + A.at(i, 5) = -yi * markers3DPoints[i][2]; + A.at(i, 6) = -yi; + + B.at(i, 0) = -xi; + } + cv::Mat V = (A.t() * A).inv() * A.t() * B;//[r21/Ty,r22/Ty,r23/Ty,alpha*r11/Ty,alpha*r12/Ty,alpha*r13/Ty,alpha*Tx/Ty] + double TySquare = 1.0 / (pow(V.at(0, 0), 2) + pow(V.at(1, 0), 2) + pow(V.at(2, 0), 2)); + + // get the sign of Ty + double far_distance = 0.0; + int far_idx = 0; + for (size_t i = 0; i < size; i++) + { + double xi = (image2DPoints[i][0] - offset_x); + double yi = (image2DPoints[i][1] - offset_y); + double r = xi * xi + yi * yi; + if (r > far_distance) + { + far_distance = r; + far_idx = i; + } + } + // yi=y-offset_y 与Yc=r_21*x_w+r_22*y_w+r_23*z_w+Ty应该是同符号的。yi*Yc>0. + // 假设Ty>0有： yi*(Yc/Ty)>0，否则Ty>0的假设不成立。 + double sign = (image2DPoints[far_idx][1] - offset_y) * ( + V.at(0, 0) * markers3DPoints[far_idx][0] + + V.at(1, 0) * markers3DPoints[far_idx][1] + + V.at(2, 0) * markers3DPoints[far_idx][2] + + 1 + ); + double Ty = sqrt(TySquare); + if (sign < 0) + { + Ty = -Ty; + } + cv::Mat r2 = (V.rowRange(0, 3) * Ty).t(); // to row vector + double alpha = cv::norm(V.rowRange(3, 6) * Ty); + cv::Mat r1 = (V.rowRange(3, 6) * Ty / alpha).t(); // to row vector + double Tx = V.at(6, 0) * Ty / alpha; + cv::Mat r3 = r1.cross(r2); + cv::Mat R; + cv::vconcat(std::vector{r1, r2, r3}, R); + + + // compute fx,fy,Tz + cv::Mat AA = cv::Mat_(size, 2); + cv::Mat BB = cv::Mat_(size, 1); + for (size_t i = 0; i < size; i++) + { + cv::Mat matp3d = (cv::Mat_(1, 3) << markers3DPoints[i][0], + markers3DPoints[i][1], markers3DPoints[i][2]); + double xi = image2DPoints[i][0] - offset_x; + double yi = image2DPoints[i][1] - offset_y; + + AA.at(i, 0) = r2.dot(matp3d) + Ty; + AA.at(i, 1) = -yi; + BB.at(i, 0) = yi * r3.dot(matp3d); + + } + + cv::Mat matKyTz = (AA.t() * AA).inv() * (AA.t() * BB); + double ky = matKyTz.at(0, 0); + double Tz = matKyTz.at(1, 0); + double kx = ky * alpha; + + // 固定SID标定内参 + double f = m_CarmTransformParameters.f; + double dx = m_CarmTransformParameters.f / kx; + double dy = m_CarmTransformParameters.f / ky; + + cv::Mat T = (cv::Mat_(3, 1) << Tx, Ty, Tz); + cv::Mat camera_pose = -R.t() * T; + + // the origin of pixel coordinate located in the left-up corner, which is on the negitive side of X and Y axis. + cv::Mat origin_camera_coordinate = (cv::Mat_(3, 1) << 0.0 - offset_x * dx, 0.0 - offset_y * dy, f); + cv::Mat image_origin = R.t() * (origin_camera_coordinate - T); + + + // TODO: 输出参数还没有完全输出 + m_CarmTransformParameters.f = f; + m_CarmTransformParameters.kx = kx; + m_CarmTransformParameters.ky = ky; + m_CarmTransformParameters.u0 = offset_x; + m_CarmTransformParameters.v0 = offset_y; + m_CarmTransformParameters.rotation_matrix = R; + m_CarmTransformParameters.translation_vector = T; + m_CarmTransformParameters.camera_pose = camera_pose; + m_CarmTransformParameters.image_spacing = cv::Vec3d(dx, dy, 1); + m_CarmTransformParameters.image_origin = cv::Vec3d( + image_origin.at(0, 0), + image_origin.at(1, 0), + image_origin.at(2, 0) + ); + + // get normal vector of image plane + cv::Mat oz = (cv::Mat_(3, 1) << 0, 0, 100); + oz = (m_CarmTransformParameters.rotation_matrix.t() * (oz - m_CarmTransformParameters.translation_vector)); + cv::Mat normalMat = (camera_pose - oz) / cv::norm(camera_pose - oz); + m_CarmTransformParameters.normal_vector_of_plane = cv::Vec3d( + normalMat.at(0, 0), + normalMat.at(1, 0), + normalMat.at(2, 0) + ); + + return true; +} + +// 优化dx,dy,Tz,k1 +void RefineTsai(std::vector& markers3DPoints, std::vector& image2DPoints) +{ + double tx = m_CarmTransformParameters.translation_vector.at(0, 0); + double ty = m_CarmTransformParameters.translation_vector.at(1, 0); + + auto r1 = m_CarmTransformParameters.rotation_matrix.rowRange(0, 1); + auto r2 = m_CarmTransformParameters.rotation_matrix.rowRange(1, 2); + auto r3 = m_CarmTransformParameters.rotation_matrix.rowRange(2, 3); + std::vector r1p, r2p, r3p; + std::transform(markers3DPoints.begin(), markers3DPoints.end(), std::back_inserter(r1p), + [=](auto p) {return r1.at(0, 0) * p[0] + r1.at(0, 1) * p[1] + r1.at(0, 2) * p[2]; }); + std::transform(markers3DPoints.begin(), markers3DPoints.end(), std::back_inserter(r2p), + [=](auto p) {return r2.at(0, 0) * p[0] + r2.at(0, 1) * p[1] + r2.at(0, 2) * p[2]; }); + std::transform(markers3DPoints.begin(), markers3DPoints.end(), std::back_inserter(r3p), + [=](auto p) {return r3.at(0, 0) * p[0] + r3.at(0, 1) * p[1] + r3.at(0, 2) * p[2]; }); + + + auto obj = [=](const ppx::MatrixS<4, 1>& x) + { + double k1 = x[0]; + double dx = x[1]; + double dy = x[2]; + double tz = x[3]; + double value = 0, vx = 0, vy = 0; + for (size_t i = 0; i < markers3DPoints.size(); i++) + { + double xc = (image2DPoints[i][0] - m_CarmTransformParameters.u0) * dx; + double yc = (image2DPoints[i][1] - m_CarmTransformParameters.v0) * dy; + double r2 = xc * xc + yc * yc; + vx += std::pow(xc * (1 + k1 * r2) * (r3p[i] + tz) - m_CarmTransformParameters.f * (r1p[i] + tx), 2); + vy += std::pow(yc * (1 + k1 * r2) * (r3p[i] + tz) - m_CarmTransformParameters.f * (r2p[i] + ty), 2); + } + value = std::sqrt(vx) + std::sqrt(vy); + + //std::cout << "Value = " << value; + //std::cout << "\tk1 = " << k1; + //std::cout << "\tdx = " << dx; + //std::cout << "\tdy = " << dy; + //std::cout << "\ttz = " << tz << std::endl; + + return value; + }; + ppx::MatrixS<4, 1> x0{ 0.0, + (double)m_CarmTransformParameters.image_spacing[0], + (double)m_CarmTransformParameters.image_spacing[1], + m_CarmTransformParameters.translation_vector.at(2, 0) + }; + auto res = ppx::fminunc(obj, x0); + std::cout << res.x.T() << std::endl; + m_CarmTransformParameters.k1 = res.x[0]; + m_CarmTransformParameters.image_spacing = cv::Vec3d(res.x[1], res.x[2], 1); + m_CarmTransformParameters.translation_vector.at(2, 0) = res.x[3]; + m_CarmTransformParameters.kx = m_CarmTransformParameters.f / res.x[1]; + m_CarmTransformParameters.ky = m_CarmTransformParameters.f / res.x[2]; +} + +// 优化dx,dy,Tz,k1,u0,v0, 按Tsai论文中的公式 X_u=X_d+δ_x +void RefineTsai2(std::vector& markers3DPoints, std::vector& image2DPoints) +{ + double tx = m_CarmTransformParameters.translation_vector.at(0, 0); + double ty = m_CarmTransformParameters.translation_vector.at(1, 0); + + auto r1 = m_CarmTransformParameters.rotation_matrix.rowRange(0, 1); + auto r2 = m_CarmTransformParameters.rotation_matrix.rowRange(1, 2); + auto r3 = m_CarmTransformParameters.rotation_matrix.rowRange(2, 3); + std::vector r1p, r2p, r3p; + std::transform(markers3DPoints.begin(), markers3DPoints.end(), std::back_inserter(r1p), + [=](auto p) {return r1.at(0, 0) * p[0] + r1.at(0, 1) * p[1] + r1.at(0, 2) * p[2]; }); + std::transform(markers3DPoints.begin(), markers3DPoints.end(), std::back_inserter(r2p), + [=](auto p) {return r2.at(0, 0) * p[0] + r2.at(0, 1) * p[1] + r2.at(0, 2) * p[2]; }); + std::transform(markers3DPoints.begin(), markers3DPoints.end(), std::back_inserter(r3p), + [=](auto p) {return r3.at(0, 0) * p[0] + r3.at(0, 1) * p[1] + r3.at(0, 2) * p[2]; }); + + + auto obj = [=](const ppx::MatrixS<6, 1>& x) + { + double k1 = x[0]; + double dx = x[1]; + double dy = x[2]; + double tz = x[3]; + double u0 = x[4]; + double v0 = x[5]; + double value = 0, vx = 0, vy = 0; + for (size_t i = 0; i < markers3DPoints.size(); i++) + { + double xc = (image2DPoints[i][0] - u0) * dx; + double yc = (image2DPoints[i][1] - v0) * dy; + double r2 = xc * xc + yc * yc; + vx += std::pow(xc * (1 + k1 * r2) * (r3p[i] + tz) - m_CarmTransformParameters.f * (r1p[i] + tx), 2); + vy += std::pow(yc * (1 + k1 * r2) * (r3p[i] + tz) - m_CarmTransformParameters.f * (r2p[i] + ty), 2); + } + value = std::sqrt(vx) + std::sqrt(vy); + + //std::cout << "Value = " << value; + //std::cout << "\tk1 = " << k1; + //std::cout << "\tdx = " << dx; + //std::cout << "\tdy = " << dy; + //std::cout << "\ttz = " << tz << std::endl; + + return value; + }; + ppx::MatrixS<6, 1> x0{ 0.0, + (double)m_CarmTransformParameters.image_spacing[0], + (double)m_CarmTransformParameters.image_spacing[1], + m_CarmTransformParameters.translation_vector.at(2, 0), + m_CarmTransformParameters.u0, + m_CarmTransformParameters.v0 + }; + auto res = ppx::fminunc(obj, x0); + std::cout << res.x.T() << std::endl; + m_CarmTransformParameters.k1 = res.x[0]; + m_CarmTransformParameters.image_spacing = cv::Vec3d(res.x[1], res.x[2], 1); + m_CarmTransformParameters.translation_vector.at(2, 0) = res.x[3]; + m_CarmTransformParameters.kx = m_CarmTransformParameters.f / res.x[1]; + m_CarmTransformParameters.ky = m_CarmTransformParameters.f / res.x[2]; + + m_CarmTransformParameters.u0 = res.x[4]; + m_CarmTransformParameters.v0 = res.x[5]; +} + + +// 优化dx,dy,Tz,k1,u0,v0, 按OpenCV公式 X_d=X_u+δ_x +void RefineTsai3(std::vector& markers3DPoints, std::vector& image2DPoints) +{ + double tx = m_CarmTransformParameters.translation_vector.at(0, 0); + double ty = m_CarmTransformParameters.translation_vector.at(1, 0); + + auto r1 = m_CarmTransformParameters.rotation_matrix.rowRange(0, 1); + auto r2 = m_CarmTransformParameters.rotation_matrix.rowRange(1, 2); + auto r3 = m_CarmTransformParameters.rotation_matrix.rowRange(2, 3); + std::vector r1p, r2p, r3p; + std::transform(markers3DPoints.begin(), markers3DPoints.end(), std::back_inserter(r1p), + [=](auto p) {return r1.at(0, 0) * p[0] + r1.at(0, 1) * p[1] + r1.at(0, 2) * p[2]; }); + std::transform(markers3DPoints.begin(), markers3DPoints.end(), std::back_inserter(r2p), + [=](auto p) {return r2.at(0, 0) * p[0] + r2.at(0, 1) * p[1] + r2.at(0, 2) * p[2]; }); + std::transform(markers3DPoints.begin(), markers3DPoints.end(), std::back_inserter(r3p), + [=](auto p) {return r3.at(0, 0) * p[0] + r3.at(0, 1) * p[1] + r3.at(0, 2) * p[2]; }); + + + auto obj = [=](const ppx::MatrixS<6, 1>& x) + { + double k1 = x[0]; + double dx = x[1]; + double dy = x[2]; + double tz = x[3]; + double u0 = x[4]; + double v0 = x[5]; + + double value = 0; + for (size_t i = 0; i < markers3DPoints.size(); i++) + { + double xu = (r1p[i] + tx) / (r3p[i] + tz); + double yu = (r2p[i] + ty) / (r3p[i] + tz); + double rs = xu * xu + yu * yu; + + double x_pie = xu * (1 + k1 * rs); + double y_pie = yu * (1 + k1 * rs); + + double u = m_CarmTransformParameters.f / dx * x_pie + u0; + double v = m_CarmTransformParameters.f / dy * y_pie + v0; + + value += hypot(u - image2DPoints[i][0], v - image2DPoints[i][1]); + } + return value; + }; + + ppx::MatrixS<6, 1> x0{ // k1,dx,dy,Tz,u0,v0 + 0.0, + (double)m_CarmTransformParameters.image_spacing[0], + (double)m_CarmTransformParameters.image_spacing[1], + m_CarmTransformParameters.translation_vector.at(2,0), + m_CarmTransformParameters.u0, + m_CarmTransformParameters.v0 + }; + auto res = ppx::fminunc(obj, x0); + std::cout << res.x.T() << std::endl; + m_CarmTransformParameters.k1 = res.x[0]; + m_CarmTransformParameters.image_spacing = cv::Vec3d(res.x[1], res.x[2], 1); + m_CarmTransformParameters.translation_vector.at(2, 0) = res.x[3]; + m_CarmTransformParameters.kx = m_CarmTransformParameters.f / res.x[1]; + m_CarmTransformParameters.ky = m_CarmTransformParameters.f / res.x[2]; + + m_CarmTransformParameters.u0 = res.x[4]; + m_CarmTransformParameters.v0 = res.x[5]; +} + + + + +//优化Rx,Ry,Rz,Tx,Ty,Tz,k1,k2,p1,p2,s1,s2,按OpenCV公式 X_d=X_u+δ_x,其中Rx,Ry,Rz是旋转角度 +void RefineTsai4(std::vector& markers3DPoints, std::vector& image2DPoints) +{ + auto obj = [=](ppx::MatrixS<12, 1> x) { + double _Rx = x[0]; + double _Ry = x[1]; + double _Rz = x[2]; + + double tx = x[3]; + double ty = x[4]; + double tz = x[5]; + + double k1 = x[6]; + double k2 = x[7]; + + double p1 = x[8]; + double p2 = x[9]; + + double s1 = x[10]; + double s3 = x[11]; + + double value = 0.0; + cv::Mat rm = angle2mat(_Rx, _Ry, _Rz); + for (size_t i = 0; i < markers3DPoints.size(); i++) + { + cv::Vec3f p = markers3DPoints[i]; + double zc = rm.at(2, 0)* p[0] + rm.at(2, 1) * p[1] + rm.at(2, 2) * p[2] + tz; + double xu = (rm.at(0, 0) * p[0] + rm.at(0, 1) * p[1] + rm.at(0, 2) * p[2] + tx) / zc; + double yu = (rm.at(1, 0) * p[0] + rm.at(1, 1) * p[1] + rm.at(1, 2) * p[2] + ty) / zc; + double rs = xu * xu + yu * yu; + + double x_pie = xu * (1 + k1 * rs + k2 * rs * rs) + 2 * p1 * xu * yu + p2 * (rs + 2 * xu * xu) + s1 * rs; + double y_pie = yu * (1 + k1 * rs + k2 * rs * rs) + p1 * (rs + 2 * yu * yu) + 2 * p2 * xu * yu + s3 * rs; + + double u = m_CarmTransformParameters.kx * x_pie + m_CarmTransformParameters.u0; + double v = m_CarmTransformParameters.ky * y_pie + m_CarmTransformParameters.v0; + + value += (u - image2DPoints[i][0]) * (u - image2DPoints[i][0]) + (v - image2DPoints[i][1]) * (v - image2DPoints[i][1]); + } + return value; + }; + + std::array angles = mat2angle(m_CarmTransformParameters.rotation_matrix); + ppx::MatrixS<12, 1> x0 = { + angles[0], + angles[1], + angles[2], + m_CarmTransformParameters.translation_vector.at(0,0), + m_CarmTransformParameters.translation_vector.at(1,0), + m_CarmTransformParameters.translation_vector.at(2,0), + m_CarmTransformParameters.k1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + }; + auto res = ppx::fminunc(obj, x0); + std::cout << res.x.T() << std::endl; + + m_CarmTransformParameters.rotation_matrix = angle2mat(res.x[0], res.x[1], res.x[2]); + m_CarmTransformParameters.translation_vector = (cv::Mat_(3, 1) << res.x[3], res.x[4], res.x[5]); + m_CarmTransformParameters.k1 = res.x[6]; + m_CarmTransformParameters.k2 = res.x[7]; + m_CarmTransformParameters.p1 = res.x[8]; + m_CarmTransformParameters.p2 = res.x[9]; + m_CarmTransformParameters.s1 = res.x[10]; + m_CarmTransformParameters.s3 = res.x[11]; + + +} + +// 优化: dx,dy,u0,v0 +void RefineTsai5(std::vector& markers3DPoints, std::vector& image2DPoints) +{ + auto r1 = m_CarmTransformParameters.rotation_matrix.rowRange(0, 1); + auto r2 = m_CarmTransformParameters.rotation_matrix.rowRange(1, 2); + auto r3 = m_CarmTransformParameters.rotation_matrix.rowRange(2, 3); + std::vector r1p, r2p, r3p; + std::transform(markers3DPoints.begin(), markers3DPoints.end(), std::back_inserter(r1p), + [=](auto p) {return r1.at(0, 0) * p[0] + r1.at(0, 1) * p[1] + r1.at(0, 2) * p[2]; }); + std::transform(markers3DPoints.begin(), markers3DPoints.end(), std::back_inserter(r2p), + [=](auto p) {return r2.at(0, 0) * p[0] + r2.at(0, 1) * p[1] + r2.at(0, 2) * p[2]; }); + std::transform(markers3DPoints.begin(), markers3DPoints.end(), std::back_inserter(r3p), + [=](auto p) {return r3.at(0, 0) * p[0] + r3.at(0, 1) * p[1] + r3.at(0, 2) * p[2]; }); + + + auto obj = [=](ppx::MatrixS<4, 1> x) { + double dx = x[0]; + double dy = x[1]; + double u0 = x[2]; + double v0 = x[3]; + + double value = 0.0; + + double tx = m_CarmTransformParameters.translation_vector.at(0, 0); + double ty = m_CarmTransformParameters.translation_vector.at(1, 0); + double tz = m_CarmTransformParameters.translation_vector.at(2, 0); + + double k1 = m_CarmTransformParameters.k1; + double k2 = m_CarmTransformParameters.k2; + + double p1 = m_CarmTransformParameters.p1; + double p2 = m_CarmTransformParameters.p2; + + double s1 = m_CarmTransformParameters.s1; + double s3 = m_CarmTransformParameters.s3; + + for (size_t i = 0; i < markers3DPoints.size(); i++) + { + double xu = (r1p[i] + tx) / (r3p[i] + tz); + double yu = (r2p[i] + ty) / (r3p[i] + tz); + + double rs = xu * xu + yu * yu; + + double x_pie = xu * (1 + k1 * rs + k2 * rs * rs) + 2 * p1 * xu * yu + p2 * (rs + 2 * xu * xu) + s1 * rs; + double y_pie = yu * (1 + k1 * rs + k2 * rs * rs) + p1 * (rs + 2 * yu * yu) + 2 * p2 * xu * yu + s3 * rs; + + double u = m_CarmTransformParameters.f / dx * x_pie + u0; + double v = m_CarmTransformParameters.f / dy * y_pie + v0; + + value += hypot(u - image2DPoints[i][0], v - image2DPoints[i][1]); + } + return value; + }; + + ppx::MatrixS<4, 1> x0 = { + (double)m_CarmTransformParameters.image_spacing[0], + (double)m_CarmTransformParameters.image_spacing[1], + m_CarmTransformParameters.u0, + m_CarmTransformParameters.v0 + }; + auto res = ppx::fminunc(obj, x0); + std::cout << res.x.T() << std::endl; + + m_CarmTransformParameters.image_spacing[0] = res.x[0]; + m_CarmTransformParameters.image_spacing[1] = res.x[1]; + + m_CarmTransformParameters.kx = m_CarmTransformParameters.f / res.x[0]; + m_CarmTransformParameters.ky = m_CarmTransformParameters.f / res.x[1]; + + m_CarmTransformParameters.u0 = res.x[2]; + m_CarmTransformParameters.v0 = res.x[3]; + +} + +// 优化除了u0,v0以外的所有参数：rx,ry,rz,tx,ty,tz,k1,k2,p1,p2,s1,s3,dx,dy +void RefineTsai6(std::vector& markers3DPoints, std::vector& image2DPoints) +{ + auto obj = [=](ppx::MatrixS<14, 1> x) { + double _Rx = x[0]; + double _Ry = x[1]; + double _Rz = x[2]; + + double tx = x[3]; + double ty = x[4]; + double tz = x[5]; + + double k1 = x[6]; + double k2 = x[7]; + + double p1 = x[8]; + double p2 = x[9]; + + double s1 = x[10]; + double s3 = x[11]; + + double dx = x[12]; + double dy = x[13]; + + double value = 0.0; + cv::Mat rm = angle2mat(_Rx, _Ry, _Rz); + for (size_t i = 0; i < markers3DPoints.size(); i++) + { + cv::Vec3f p = markers3DPoints[i]; + double zc = rm.at(2, 0) * p[0] + rm.at(2, 1) * p[1] + rm.at(2, 2) * p[2] + tz; + double xu = (rm.at(0, 0) * p[0] + rm.at(0, 1) * p[1] + rm.at(0, 2) * p[2] + tx) / zc; + double yu = (rm.at(1, 0) * p[0] + rm.at(1, 1) * p[1] + rm.at(1, 2) * p[2] + ty) / zc; + double rs = xu * xu + yu * yu; + + double x_pie = xu * (1 + k1 * rs + k2 * rs * rs) + 2 * p1 * xu * yu + p2 * (rs + 2 * xu * xu) + s1 * rs; + double y_pie = yu * (1 + k1 * rs + k2 * rs * rs) + p1 * (rs + 2 * yu * yu) + 2 * p2 * xu * yu + s3 * rs; + + double u = m_CarmTransformParameters.f / dx * x_pie + m_CarmTransformParameters.u0; + double v = m_CarmTransformParameters.f / dy * y_pie + m_CarmTransformParameters.v0; + + value += (u - image2DPoints[i][0]) * (u - image2DPoints[i][0]) + (v - image2DPoints[i][1]) * (v - image2DPoints[i][1]); + } + return value; + }; + + std::array angles = mat2angle(m_CarmTransformParameters.rotation_matrix); + ppx::MatrixS<14, 1> x0 = { + angles[0], + angles[1], + angles[2], + m_CarmTransformParameters.translation_vector.at(0,0), + m_CarmTransformParameters.translation_vector.at(1,0), + m_CarmTransformParameters.translation_vector.at(2,0), + m_CarmTransformParameters.k1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + m_CarmTransformParameters.image_spacing[0], + m_CarmTransformParameters.image_spacing[1] + }; + auto res = ppx::fminunc(obj, x0); + std::cout << res.x.T() << std::endl; + + m_CarmTransformParameters.rotation_matrix = angle2mat(res.x[0], res.x[1], res.x[2]); + m_CarmTransformParameters.translation_vector = (cv::Mat_(3, 1) << res.x[3], res.x[4], res.x[5]); + m_CarmTransformParameters.k1 = res.x[6]; + m_CarmTransformParameters.k2 = res.x[7]; + m_CarmTransformParameters.p1 = res.x[8]; + m_CarmTransformParameters.p2 = res.x[9]; + m_CarmTransformParameters.s1 = res.x[10]; + m_CarmTransformParameters.s3 = res.x[11]; + + m_CarmTransformParameters.image_spacing[0] = res.x[12]; + m_CarmTransformParameters.image_spacing[1] = res.x[13]; + + m_CarmTransformParameters.kx = m_CarmTransformParameters.f / res.x[12]; + m_CarmTransformParameters.ky = m_CarmTransformParameters.f / res.x[13]; + +} + + +// 优化所有参数：rx,ry,rz,tx,ty,tz,k1,k2,p1,p2,s1,s3,dx,dy,u0,v0 +void RefineTsai7(std::vector& markers3DPoints, std::vector& image2DPoints) +{ + auto obj = [=](ppx::MatrixS<16, 1> x) { + double _Rx = x[0]; + double _Ry = x[1]; + double _Rz = x[2]; + + double tx = x[3]; + double ty = x[4]; + double tz = x[5]; + + double k1 = x[6]; + double k2 = x[7]; + + double p1 = x[8]; + double p2 = x[9]; + + double s1 = x[10]; + double s3 = x[11]; + + double dx = x[12]; + double dy = x[13]; + + double u0 = x[14]; + double v0 = x[15]; + + double value = 0.0; + cv::Mat rm = angle2mat(_Rx, _Ry, _Rz); + for (size_t i = 0; i < markers3DPoints.size(); i++) + { + cv::Vec3f p = markers3DPoints[i]; + double zc = rm.at(2, 0) * p[0] + rm.at(2, 1) * p[1] + rm.at(2, 2) * p[2] + tz; + double xu = (rm.at(0, 0) * p[0] + rm.at(0, 1) * p[1] + rm.at(0, 2) * p[2] + tx) / zc; + double yu = (rm.at(1, 0) * p[0] + rm.at(1, 1) * p[1] + rm.at(1, 2) * p[2] + ty) / zc; + double rs = xu * xu + yu * yu; + + double x_pie = xu * (1 + k1 * rs + k2 * rs * rs) + 2 * p1 * xu * yu + p2 * (rs + 2 * xu * xu) + s1 * rs; + double y_pie = yu * (1 + k1 * rs + k2 * rs * rs) + p1 * (rs + 2 * yu * yu) + 2 * p2 * xu * yu + s3 * rs; + + double u = m_CarmTransformParameters.f / dx * x_pie + u0; + double v = m_CarmTransformParameters.f / dy * y_pie + v0; + + value += (u - image2DPoints[i][0]) * (u - image2DPoints[i][0]) + (v - image2DPoints[i][1]) * (v - image2DPoints[i][1]); + } + return value; + }; + + std::array angles = mat2angle(m_CarmTransformParameters.rotation_matrix); + ppx::MatrixS<16, 1> x0 = { + angles[0], + angles[1], + angles[2], + m_CarmTransformParameters.translation_vector.at(0,0), + m_CarmTransformParameters.translation_vector.at(1,0), + m_CarmTransformParameters.translation_vector.at(2,0), + m_CarmTransformParameters.k1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + m_CarmTransformParameters.image_spacing[0], + m_CarmTransformParameters.image_spacing[1], + m_CarmTransformParameters.u0, + m_CarmTransformParameters.v0 + }; + auto res = ppx::fminunc(obj, x0); + std::cout << res.x.T() << std::endl; + + m_CarmTransformParameters.rotation_matrix = angle2mat(res.x[0], res.x[1], res.x[2]); + m_CarmTransformParameters.translation_vector = (cv::Mat_(3, 1) << res.x[3], res.x[4], res.x[5]); + m_CarmTransformParameters.k1 = res.x[6]; + m_CarmTransformParameters.k2 = res.x[7]; + m_CarmTransformParameters.p1 = res.x[8]; + m_CarmTransformParameters.p2 = res.x[9]; + m_CarmTransformParameters.s1 = res.x[10]; + m_CarmTransformParameters.s3 = res.x[11]; + + m_CarmTransformParameters.image_spacing[0] = res.x[12]; + m_CarmTransformParameters.image_spacing[1] = res.x[13]; + + m_CarmTransformParameters.kx = m_CarmTransformParameters.f / res.x[12]; + m_CarmTransformParameters.ky = m_CarmTransformParameters.f / res.x[13]; + + m_CarmTransformParameters.u0 = res.x[14]; + m_CarmTransformParameters.v0 = res.x[15]; + +} + +void test_1(); + +void test_2(); + +int main___() +{ + + test_1(); + + getchar(); + + std::vector points3D; + std::vector points2D; + + set_inner_data(points3D, points2D); + + std::cout << "\nCalibration by Tsai without any distortion:=========================" << std::endl; + Tsai(points3D, points2D); + std::cout << m_CarmTransformParameters << std::endl; + ProjectErrorStat(points3D, points2D); + + std::cout << "\nCalibration by Tsai only k1 distortion:=============================" << std::endl; + RefineTsai(points3D, points2D); + std::cout << m_CarmTransformParameters << std::endl; + ProjectErrorStat(points3D, points2D); + + //set_data(points3D, points2D); + + //std::cout << "\nCalibration by Tsai with k1 distortion and u0,v0 offset:============" << std::endl; + //RefineTsai2(points3D, points2D); + //std::cout << m_CarmTransformParameters << std::endl; + //ProjectErrorStat(points3D, points2D); + + set_data(points3D, points2D); + std::cout << "\nCalibration by Tsai with k1 distortion and u0,v0 optimization:============" << std::endl; + RefineTsai3(points3D, points2D); + std::cout << m_CarmTransformParameters << std::endl; + ProjectErrorStatOpenCV(points3D, points2D); + + + for (size_t i = 0; i < 3; i++) + { + std::cout << "\nCalibration by Tsai with R, T and distortion optimization:============" << std::endl; + RefineTsai4(points3D, points2D); + std::cout << m_CarmTransformParameters << std::endl; + ProjectErrorStatOpenCV(points3D, points2D); + + std::cout << "\nCalibration by Tsai with dy,dy,u0,v0 optimization:============" << std::endl; + RefineTsai5(points3D, points2D); + std::cout << m_CarmTransformParameters << std::endl; + ProjectErrorStatOpenCV(points3D, points2D); + } + + + + + + + return 0; +} + + + +// CN专利方式 +void test_1() +{ + double image_w = 1024; + double image_h = 1024; + // 内参 + double f = 1011; + double dx =0.209, dy = 0.209; + double u0 = 505, v0 = 509.1; + + // 外参 + double rx = 0.001; + double ry = -0.001; + double rz = 0.002; + + double tx = 10; + double ty = -10; + double tz = 950; + + // 畸变参数 + double k1 = 5; + double k2 = 100; + double p1 = -0.01; + double p2 = -0.01; + double s1 = -0.02; + double s3 = 0.01; + + + cv::Mat camera_matrix = (cv::Mat_(3, 3) << f / dx, 0, u0, 0, f / dy, v0, 0, 0, 1); + cv::Mat camera_matrix_undistortion = (cv::Mat_(3, 3) << f / dx, 0, (image_w - 1.0) / 2, 0, f / dy, (image_h - 1.0) / 2, 0, 0, 1); + cv::Mat rotation = angle2mat(rx, ry, rz); + cv::Mat rvec = (cv::Mat_(3, 1) << rz, ry, rz); + cv::Mat translation = (cv::Mat_(3, 1) << tx, ty, tz); + + // k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4 + cv::Mat distortion_coeff = (cv::Mat_(12, 1) << k1, k2, p1, p2, 0, 0, 0, 0, s1, 0, s3, 0); + + std::vector points3D = + { +#include "marker3d.data" + }; + + std::vector points2D, points2D_undistortion; + cv::projectPoints(points3D, rvec, translation, camera_matrix, distortion_coeff, points2D); + cv::projectPoints(points3D, rvec, translation, camera_matrix_undistortion, cv::Mat(), points2D_undistortion); + + cv::Mat preview(1024, 1024, CV_8UC3); + for (size_t i = 0; i < points2D.size(); i++) + { + cv::circle(preview, cv::Point(points2D[i]), 10, cv::Scalar(0, 0, 255), 2); + cv::circle(preview, cv::Point(points2D_undistortion[i]), 10, cv::Scalar(255, 0, 255), 2); + } + cv::imshow("preview", preview); + cv::waitKey(0); + + + + + + + + std::cout << "\nCalibration by Tsai without any distortion:=========================" << std::endl; + Tsai(points3D, points2D); + std::cout << m_CarmTransformParameters << std::endl; + ProjectErrorStat(points3D, points2D); + + std::cout << "\nCalibration by Tsai only k1 distortion:=============================" << std::endl; + RefineTsai(points3D, points2D); + std::cout << m_CarmTransformParameters << std::endl; + ProjectErrorStat(points3D, points2D); + + //set_data(points3D, points2D); + + //std::cout << "\nCalibration by Tsai with k1 distortion and u0,v0 offset:============" << std::endl; + //RefineTsai2(points3D, points2D); + //std::cout << m_CarmTransformParameters << std::endl; + //ProjectErrorStat(points3D, points2D); + + //set_data(points3D, points2D); + std::cout << "\nCalibration by Tsai with k1 distortion and u0,v0 optimization:============" << std::endl; + RefineTsai3(points3D, points2D); + std::cout << m_CarmTransformParameters << std::endl; + ProjectErrorStatOpenCV(points3D, points2D); + + + for (size_t i = 0; i < 3; i++) + { + std::cout << "\nCalibration by Tsai with R, T and distortion optimization:============" << std::endl; + RefineTsai4(points3D, points2D); + std::cout << m_CarmTransformParameters << std::endl; + ProjectErrorStatOpenCV(points3D, points2D); + + std::cout << "\nCalibration by Tsai with dy,dy,u0,v0 optimization:============" << std::endl; + RefineTsai5(points3D, points2D); + std::cout << m_CarmTransformParameters << std::endl; + ProjectErrorStatOpenCV(points3D, points2D); + } + + std::vector points2D_u; + cv::Mat d = (cv::Mat_(12, 1) << m_CarmTransformParameters.k1, m_CarmTransformParameters.k2, // k1,k2 + m_CarmTransformParameters.p1, m_CarmTransformParameters.p2, // p1,p2 + 0, 0, 0, 0, // k3,k4,k5,k6 + m_CarmTransformParameters.s1, 0, m_CarmTransformParameters.s3, 0); // s1,s2,s3,s4 + + cv::Mat camera_matrix_u = + (cv::Mat_(3, 3) << m_CarmTransformParameters.kx, 0, m_CarmTransformParameters.u0, + 0, m_CarmTransformParameters.ky, m_CarmTransformParameters.v0, + 0, 0, 1); + + cv::undistortPoints(points2D, points2D_u, camera_matrix_u, d, cv::Mat(), camera_matrix_undistortion); + + auto angles = mat2angle(m_CarmTransformParameters.rotation_matrix); + rvec = (cv::Mat_(3, 1) << angles[0], angles[1], angles[2]); + cv::projectPoints(points3D, rvec, m_CarmTransformParameters.translation_vector, camera_matrix_undistortion, cv::Mat(), points2D_undistortion); + + cv::Mat preview2(1024, 1024, CV_8UC3); + for (size_t i = 0; i < points2D.size(); i++) + { + cv::circle(preview2, cv::Point(points2D_u[i]), 10, cv::Scalar(0, 0, 255), -1); + cv::circle(preview2, cv::Point(points2D_undistortion[i]), 10, cv::Scalar(255, 0, 255), 2); + std::cout << "Distance = " << cv::norm(points2D_u[i] - points2D_undistortion[i]) << std::endl; + } + cv::imshow("preview2", preview2); + cv::waitKey(0); + +} + + +// pyTsai方式 +void test_2() +{ + double image_w = 1024; + double image_h = 1024; + // 内参 + double f = 1011; + double dx = 0.209, dy = 0.209; + double u0 = 505, v0 = 509.1; + + // 外参 + double rx = 0.001; + double ry = -0.001; + double rz = 0.002; + + double tx = 10; + double ty = -10; + double tz = 950; + + // 畸变参数 + double k1 = 5; + double k2 = 100; + double p1 = -0.01; + double p2 = -0.01; + double s1 = -0.02; + double s3 = 0.01; + + + cv::Mat camera_matrix = (cv::Mat_(3, 3) << f / dx, 0, u0, 0, f / dy, v0, 0, 0, 1); + cv::Mat camera_matrix_undistortion = (cv::Mat_(3, 3) << f / dx, 0, (image_w - 1.0) / 2, 0, f / dy, (image_h - 1.0) / 2, 0, 0, 1); + cv::Mat rotation = angle2mat(rx, ry, rz); + cv::Mat rvec = (cv::Mat_(3, 1) << rz, ry, rz); + cv::Mat translation = (cv::Mat_(3, 1) << tx, ty, tz); + + // k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4 + cv::Mat distortion_coeff = (cv::Mat_(12, 1) << k1, k2, p1, p2, 0, 0, 0, 0, s1, 0, s3, 0); + + std::vector points3D = + { +#include "marker3d.data" + }; + + std::vector points2D, points2D_undistortion; + cv::projectPoints(points3D, rvec, translation, camera_matrix, distortion_coeff, points2D); + cv::projectPoints(points3D, rvec, translation, camera_matrix_undistortion, cv::Mat(), points2D_undistortion); + + cv::Mat preview(1024, 1024, CV_8UC3); + for (size_t i = 0; i < points2D.size(); i++) + { + cv::circle(preview, cv::Point(points2D[i]), 10, cv::Scalar(0, 0, 255), 2); + cv::circle(preview, cv::Point(points2D_undistortion[i]), 10, cv::Scalar(255, 0, 255), 2); + } + cv::imshow("preview", preview); + cv::waitKey(0); + + + + std::cout << "\nCalibration by Tsai without any distortion:=========================" << std::endl; + Tsai(points3D, points2D); + std::cout << m_CarmTransformParameters << std::endl; + ProjectErrorStat(points3D, points2D); + + std::cout << "\nCalibration by Tsai only k1 distortion:=============================" << std::endl; + RefineTsai(points3D, points2D); + std::cout << m_CarmTransformParameters << std::endl; + ProjectErrorStat(points3D, points2D); + + + std::cout << "\nCalibration by Tsai without optical center offset:=============================" << std::endl; + RefineTsai6(points3D, points2D); + std::cout << m_CarmTransformParameters << std::endl; + ProjectErrorStatOpenCV(points3D, points2D); + + + std::cout << "\nCalibration by Tsai full optimazation:=============================" << std::endl; + RefineTsai7(points3D, points2D); + std::cout << m_CarmTransformParameters << std::endl; + ProjectErrorStatOpenCV(points3D, points2D); + + + std::vector points2D_u; + cv::Mat d = (cv::Mat_(12, 1) << m_CarmTransformParameters.k1, m_CarmTransformParameters.k2, // k1,k2 + m_CarmTransformParameters.p1, m_CarmTransformParameters.p2, // p1,p2 + 0, 0, 0, 0, // k3,k4,k5,k6 + m_CarmTransformParameters.s1, 0, m_CarmTransformParameters.s3, 0); // s1,s2,s3,s4 + + cv::Mat camera_matrix_u = + (cv::Mat_(3, 3) << m_CarmTransformParameters.kx, 0, m_CarmTransformParameters.u0, + 0, m_CarmTransformParameters.ky, m_CarmTransformParameters.v0, + 0, 0, 1); + + cv::undistortPoints(points2D, points2D_u, camera_matrix_u, d, cv::Mat(), camera_matrix_undistortion); + + auto angles = mat2angle(m_CarmTransformParameters.rotation_matrix); + rvec = (cv::Mat_(3, 1) << angles[0], angles[1], angles[2]); + cv::projectPoints(points3D, rvec, m_CarmTransformParameters.translation_vector, camera_matrix_undistortion, cv::Mat(), points2D_undistortion); + + cv::Mat preview2(1024, 1024, CV_8UC3); + for (size_t i = 0; i < points2D.size(); i++) + { + cv::circle(preview2, cv::Point(points2D_u[i]), 10, cv::Scalar(0, 0, 255), -1); + cv::circle(preview2, cv::Point(points2D_undistortion[i]), 10, cv::Scalar(255, 0, 255), 2); + std::cout << "Distance = " << cv::norm(points2D_u[i] - points2D_undistortion[i]) << std::endl; + } + cv::imshow("preview2", preview2); + cv::waitKey(0); + +} \ No newline at end of file diff --git a/others/TsaiCpp/TsaiCalibration.cpp.xyz b/others/TsaiCpp/TsaiCalibration.cpp.xyz new file mode 100644 index 0000000..3866355 --- /dev/null +++ b/others/TsaiCpp/TsaiCalibration.cpp.xyz @@ -0,0 +1,815 @@ + +#include "TsaiCalibration.h.xyzh" +#include "matrixs/matrixs.hpp" +#include "matrixs/optimization.hpp" + +TsaiCalibration::TsaiCalibration() +{ + m_Parameters.f = 1011; +} + +TsaiCalibration::~TsaiCalibration() +{ +} + +cv::Mat TsaiCalibration::Angle2RotationMat(const double& Rx, const double& Ry, const double& Rz) +{ + double sa = sin(Rx); + double ca = cos(Rx); + double sb = sin(Ry); + double cb = cos(Ry); + double sg = sin(Rz); + double cg = cos(Rz); + + cv::Mat rm(3, 3, CV_64FC1); + + rm.at(0, 0) = cb * cg; + rm.at(0, 1) = cg * sa * sb - ca * sg; + rm.at(0, 2) = sa * sg + ca * cg * sb; + rm.at(1, 0) = cb * sg; + rm.at(1, 1) = sa * sb * sg + ca * cg; + rm.at(1, 2) = ca * sb * sg - cg * sa; + rm.at(2, 0) = -sb; + rm.at(2, 1) = cb * sa; + rm.at(2, 2) = ca * cb; + + return rm; +} + +std::array TsaiCalibration::RotationMat2Angle(const cv::Mat& rm) +{ + double r1, r2, r3, r4, r5, r6, r7, sg, cg; + if (rm.empty() || rm.cols != 3 || rm.cols != 3) + { + return {}; + } + r1 = rm.at(0, 0); + r2 = rm.at(0, 1); + r3 = rm.at(0, 2); + r4 = rm.at(1, 0); + r5 = rm.at(1, 1); + r6 = rm.at(1, 2); + r7 = rm.at(2, 0); + + double Rz = atan2(r4, r1); + sg = sin(Rz); + cg = cos(Rz); + double Ry = atan2(-r7, r1 * cg + r4 * sg); + double Rx = atan2(r3 * sg - r6 * cg, r5 * cg - r2 * sg); + + return { Rx,Ry,Rz }; +} + + +bool TsaiCalibration::ThreeStepCalibration() +{ + //Step 1 + if (!Tsai(m_Points3D, m_Points2D)) // linear Tsai part + { + return false; + } + RefineTsai(m_Points3D, m_Points2D); // non-linear Tsai part + // Step 2-3 + RefineWithoutOpticalCenter(m_Points3D, m_Points2D); // non-linear optimization for internal and external parameters expect u0,v0 + RefineFullParamters(m_Points3D, m_Points2D); // non-linear optimization for all parameters + return true; +} + +bool TsaiCalibration::FiveStepCalibration() +{ + + // Step1 + if (!Tsai(m_InnerPoints3D, m_InnerPoints2D)) // linear Tsai part + { + return false; + } + RefineTsai(m_InnerPoints3D, m_InnerPoints2D); // non-linear Tsai part + RefineTsaiWithOpticalCenter(m_InnerPoints3D, m_InnerPoints2D); // non-linear Tsai part with optical center + // Step 2-3 + RefineExternalParameters(m_InnerPoints3D, m_InnerPoints2D); // non-linear optimization for external and distortion parameters with centered points + RefineIntermalParameters(m_InnerPoints3D, m_InnerPoints2D); // non-linear optimization for inner parameters with centered points + // Step 4-5 + RefineExternalParameters(m_Points3D, m_Points2D); // non-linear optimization for external and distortion parameters with all points + RefineIntermalParameters(m_Points3D, m_Points2D); // non-linear optimization for inner parameters with all points + return true; +} + +bool TsaiCalibration::Execute(CalibrationType type) +{ + if (m_Points2D.size() != m_Points3D.size() || m_InnerPoints2D.size() != m_InnerPoints3D.size()) + { + // invalid point pairs + return false; + } + + if (m_Points2D.size() < 7) + { + // insufficient point pairs + return false; + } + + if (type == CalibrationType::FiveStepOptimization && m_InnerPoints2D.size() < 7) + { + // insufficient point pairs for FiveStepOptimization + return false; + } + + + if (type == CalibrationType::ThreeStepOptimation) + { + return ThreeStepCalibration(); + } + else + { + return FiveStepCalibration(); + } +} + + +/** + * @brief Calibration with linear Tsai method, for detail please refer: + * Tsai R. A versatile camera calibration technique for high-accuracy 3D + * machine vision metrology using off-the-shelf TV cameras and lenses[J]. + * IEEE Journal on Robotics and Automation, 1987, 3(4): 323-344. + * @return +*/ +bool TsaiCalibration::Tsai(const std::vector& points3D, const std::vector& points2D) +{ + if (points3D.size() != points2D.size()) + { + return false; + } + if (points3D.size() < 7) + { + return false; + } + int size = points3D.size(); + // compute r11,r12,r13,r21,r22,r23,Tx,Ty + double offset_x = (m_Parameters.image_width - 1.0) / 2.0; + double offset_y = (m_Parameters.image_height - 1.0) / 2.0; + + cv::Vec3d spacing = m_Parameters.image_spacing; + + cv::Mat A = cv::Mat_(size, 7); + cv::Mat B = cv::Mat_(size, 1); + for (size_t i = 0; i < size; i++) + { + double xi = (points2D[i][0] - offset_x); + double yi = (points2D[i][1] - offset_y); + + A.at(i, 0) = xi * points3D[i][0]; + A.at(i, 1) = xi * points3D[i][1]; + A.at(i, 2) = xi * points3D[i][2]; + A.at(i, 3) = -yi * points3D[i][0]; + A.at(i, 4) = -yi * points3D[i][1]; + A.at(i, 5) = -yi * points3D[i][2]; + A.at(i, 6) = -yi; + + B.at(i, 0) = -xi; + } + cv::Mat V = (A.t() * A).inv() * A.t() * B;//[r21/Ty,r22/Ty,r23/Ty,alpha*r11/Ty,alpha*r12/Ty,alpha*r13/Ty,alpha*Tx/Ty] + double TySquare = 1.0 / (pow(V.at(0, 0), 2) + pow(V.at(1, 0), 2) + pow(V.at(2, 0), 2)); + + // get the sign of Ty + double far_distance = 0.0; + int far_idx = 0; + for (size_t i = 0; i < size; i++) + { + double xi = (points2D[i][0] - offset_x); + double yi = (points2D[i][1] - offset_y); + double r = xi * xi + yi * yi; + if (r > far_distance) + { + far_distance = r; + far_idx = i; + } + } + // yi=y-offset_y 与Yc=r_21*x_w+r_22*y_w+r_23*z_w+Ty应该是同符号的。yi*Yc>0. + // 假设Ty>0有： yi*(Yc/Ty)>0，否则Ty>0的假设不成立。 + double sign = (points2D[far_idx][1] - offset_y) * ( + V.at(0, 0) * points3D[far_idx][0] + + V.at(1, 0) * points3D[far_idx][1] + + V.at(2, 0) * points3D[far_idx][2] + + 1 + ); + double Ty = sqrt(TySquare); + if (sign < 0) + { + Ty = -Ty; + } + cv::Mat r2 = (V.rowRange(0, 3) * Ty).t(); // to row vector + double alpha = cv::norm(V.rowRange(3, 6) * Ty); + cv::Mat r1 = (V.rowRange(3, 6) * Ty / alpha).t(); // to row vector + double Tx = V.at(6, 0) * Ty / alpha; + cv::Mat r3 = r1.cross(r2); + cv::Mat R; + cv::vconcat(std::vector{r1, r2, r3}, R); + + + // compute fx,fy,Tz + cv::Mat AA = cv::Mat_(size, 2); + cv::Mat BB = cv::Mat_(size, 1); + for (size_t i = 0; i < size; i++) + { + cv::Mat matp3d = (cv::Mat_(1, 3) << points3D[i][0], + points3D[i][1], points3D[i][2]); + double xi = points2D[i][0] - offset_x; + double yi = points2D[i][1] - offset_y; + + AA.at(i, 0) = r2.dot(matp3d) + Ty; + AA.at(i, 1) = -yi; + BB.at(i, 0) = yi * r3.dot(matp3d); + + } + + cv::Mat matKyTz = (AA.t() * AA).inv() * (AA.t() * BB); + double ky = matKyTz.at(0, 0); + double Tz = matKyTz.at(1, 0); + double kx = ky * alpha; + + // 固定SID标定内参 + double f = m_Parameters.f; + double dx = m_Parameters.f / kx; + double dy = m_Parameters.f / ky; + + cv::Mat T = (cv::Mat_(3, 1) << Tx, Ty, Tz); + cv::Mat camera_pose = -R.t() * T; + + // the origin of pixel coordinate located in the left-up corner, which is on the negitive side of X and Y axis. + cv::Mat origin_camera_coordinate = (cv::Mat_(3, 1) << 0.0 - offset_x * dx, 0.0 - offset_y * dy, f); + cv::Mat image_origin = R.t() * (origin_camera_coordinate - T); + + + m_Parameters.f = f; + m_Parameters.kx = kx; + m_Parameters.ky = ky; + m_Parameters.u0 = offset_x; + m_Parameters.v0 = offset_y; + m_Parameters.rotation_matrix = R; + m_Parameters.translation_vector = T; + m_Parameters.image_spacing = cv::Vec3d(dx, dy, 1); + + return true; +} + +/** + * @brief Refine kx/ky, Tz and k1 for Linear Tsai Method. This is the second + * step for original paper, for detail please refer: + * Tsai R. A versatile camera calibration technique for high-accuracy 3D + * machine vision metrology using off-the-shelf TV cameras and lenses[J]. + * IEEE Journal on Robotics and Automation, 1987, 3(4): 323-344. + * @return +*/ +void TsaiCalibration::RefineTsai(const std::vector& points3D, const std::vector& points2D) +{ + double tx = m_Parameters.translation_vector.at(0, 0); + double ty = m_Parameters.translation_vector.at(1, 0); + + auto r1 = m_Parameters.rotation_matrix.rowRange(0, 1); + auto r2 = m_Parameters.rotation_matrix.rowRange(1, 2); + auto r3 = m_Parameters.rotation_matrix.rowRange(2, 3); + std::vector r1p, r2p, r3p; + std::transform(points3D.begin(), points3D.end(), std::back_inserter(r1p), + [=](auto p) {return r1.at(0, 0) * p[0] + r1.at(0, 1) * p[1] + r1.at(0, 2) * p[2]; }); + std::transform(points3D.begin(), points3D.end(), std::back_inserter(r2p), + [=](auto p) {return r2.at(0, 0) * p[0] + r2.at(0, 1) * p[1] + r2.at(0, 2) * p[2]; }); + std::transform(points3D.begin(), points3D.end(), std::back_inserter(r3p), + [=](auto p) {return r3.at(0, 0) * p[0] + r3.at(0, 1) * p[1] + r3.at(0, 2) * p[2]; }); + + + auto obj = [=](const ppx::MatrixS<4, 1>& x) + { + double k1 = x[0]; + double dx = x[1]; + double dy = x[2]; + double tz = x[3]; + double value = 0, vx = 0, vy = 0; + for (size_t i = 0; i < points3D.size(); i++) + { + double xc = (points2D[i][0] - m_Parameters.u0) * dx; + double yc = (points2D[i][1] - m_Parameters.v0) * dy; + double r2 = xc * xc + yc * yc; + vx += std::pow(xc * (1 + k1 * r2) * (r3p[i] + tz) - m_Parameters.f * (r1p[i] + tx), 2); + vy += std::pow(yc * (1 + k1 * r2) * (r3p[i] + tz) - m_Parameters.f * (r2p[i] + ty), 2); + } + value = std::sqrt(vx) + std::sqrt(vy); + + return value; + }; + ppx::MatrixS<4, 1> x0{ 0.0, + (double)m_Parameters.image_spacing[0], + (double)m_Parameters.image_spacing[1], + m_Parameters.translation_vector.at(2, 0) + }; + auto res = ppx::fminunc(obj, x0); + std::cout << res.x.T() << std::endl; + m_Parameters.k1 = res.x[0]; + m_Parameters.image_spacing = cv::Vec3d(res.x[1], res.x[2], 1); + m_Parameters.translation_vector.at(2, 0) = res.x[3]; + m_Parameters.kx = m_Parameters.f / res.x[1]; + m_Parameters.ky = m_Parameters.f / res.x[2]; +} + +/** + * @brief Non-linear optimization dx,dy,Tz,k1 and optical center (u0,v0). + * This part could be viewed as an improved Step2 Tsai calibration, since + * it optimized not only dx,dy,Tz and k1 but also u0,v0. +*/ +void TsaiCalibration::RefineTsaiWithOpticalCenter(const std::vector& points3D, const std::vector& points2D) +{ + double tx = m_Parameters.translation_vector.at(0, 0); + double ty = m_Parameters.translation_vector.at(1, 0); + + auto r1 = m_Parameters.rotation_matrix.rowRange(0, 1); + auto r2 = m_Parameters.rotation_matrix.rowRange(1, 2); + auto r3 = m_Parameters.rotation_matrix.rowRange(2, 3); + std::vector r1p, r2p, r3p; + std::transform(points3D.begin(), points3D.end(), std::back_inserter(r1p), + [=](auto p) {return r1.at(0, 0) * p[0] + r1.at(0, 1) * p[1] + r1.at(0, 2) * p[2]; }); + std::transform(points3D.begin(), points3D.end(), std::back_inserter(r2p), + [=](auto p) {return r2.at(0, 0) * p[0] + r2.at(0, 1) * p[1] + r2.at(0, 2) * p[2]; }); + std::transform(points3D.begin(), points3D.end(), std::back_inserter(r3p), + [=](auto p) {return r3.at(0, 0) * p[0] + r3.at(0, 1) * p[1] + r3.at(0, 2) * p[2]; }); + + + auto obj = [=](const ppx::MatrixS<6, 1>& x) + { + double k1 = x[0]; + double dx = x[1]; + double dy = x[2]; + double tz = x[3]; + double u0 = x[4]; + double v0 = x[5]; + + double value = 0; + for (size_t i = 0; i < points3D.size(); i++) + { + double xu = (r1p[i] + tx) / (r3p[i] + tz); + double yu = (r2p[i] + ty) / (r3p[i] + tz); + double rs = xu * xu + yu * yu; + + double x_pie = xu * (1 + k1 * rs); + double y_pie = yu * (1 + k1 * rs); + + double u = m_Parameters.f / dx * x_pie + u0; + double v = m_Parameters.f / dy * y_pie + v0; + + value += hypot(u - points2D[i][0], v - points2D[i][1]); + } + return value; + }; + + ppx::MatrixS<6, 1> x0{ // k1,dx,dy,Tz,u0,v0 + 0.0, + (double)m_Parameters.image_spacing[0], + (double)m_Parameters.image_spacing[1], + m_Parameters.translation_vector.at(2,0), + m_Parameters.u0, + m_Parameters.v0 + }; + auto res = ppx::fminunc(obj, x0); + std::cout << res.x.T() << std::endl; + m_Parameters.k1 = res.x[0]; + m_Parameters.image_spacing = cv::Vec3d(res.x[1], res.x[2], 1); + m_Parameters.translation_vector.at(2, 0) = res.x[3]; + m_Parameters.kx = m_Parameters.f / res.x[1]; + m_Parameters.ky = m_Parameters.f / res.x[2]; + + m_Parameters.u0 = res.x[4]; + m_Parameters.v0 = res.x[5]; +} + +/** + * @brief non-linear optimization for internal and external parameters expect u0,v0. + * The parameters for optimization includes: rx,ry,rz,tx,ty,tz,k1,k2,p1,p2,s1,s3,dy,dy + * @return +*/ +void TsaiCalibration::RefineWithoutOpticalCenter(const std::vector& points3D, const std::vector& points2D) +{ + auto obj = [=](ppx::MatrixS<14, 1> x) { + double _Rx = x[0]; + double _Ry = x[1]; + double _Rz = x[2]; + + double tx = x[3]; + double ty = x[4]; + double tz = x[5]; + + double k1 = x[6]; + double k2 = x[7]; + + double p1 = x[8]; + double p2 = x[9]; + + double s1 = x[10]; + double s3 = x[11]; + + double dx = x[12]; + double dy = x[13]; + + double value = 0.0; + cv::Mat rm = Angle2RotationMat(_Rx, _Ry, _Rz); + for (size_t i = 0; i < points3D.size(); i++) + { + cv::Vec3f p = points3D[i]; + double zc = rm.at(2, 0) * p[0] + rm.at(2, 1) * p[1] + rm.at(2, 2) * p[2] + tz; + double xu = (rm.at(0, 0) * p[0] + rm.at(0, 1) * p[1] + rm.at(0, 2) * p[2] + tx) / zc; + double yu = (rm.at(1, 0) * p[0] + rm.at(1, 1) * p[1] + rm.at(1, 2) * p[2] + ty) / zc; + double rs = xu * xu + yu * yu; + + double x_pie = xu * (1 + k1 * rs + k2 * rs * rs) + 2 * p1 * xu * yu + p2 * (rs + 2 * xu * xu) + s1 * rs; + double y_pie = yu * (1 + k1 * rs + k2 * rs * rs) + p1 * (rs + 2 * yu * yu) + 2 * p2 * xu * yu + s3 * rs; + + double u = m_Parameters.f / dx * x_pie + m_Parameters.u0; + double v = m_Parameters.f / dy * y_pie + m_Parameters.v0; + + value += (u - points2D[i][0]) * (u - points2D[i][0]) + (v - points2D[i][1]) * (v - points2D[i][1]); + } + return value; + }; + + std::array angles = RotationMat2Angle(m_Parameters.rotation_matrix); + // rx,ry,rz,tx,ty,tz,k1,k2,p1,p2,s1,s3,dy,dy + ppx::MatrixS<14, 1> x0 = { + angles[0], + angles[1], + angles[2], + m_Parameters.translation_vector.at(0,0), + m_Parameters.translation_vector.at(1,0), + m_Parameters.translation_vector.at(2,0), + m_Parameters.k1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + m_Parameters.image_spacing[0], + m_Parameters.image_spacing[1] + }; + auto res = ppx::fminunc(obj, x0); + std::cout << res.x.T() << std::endl; + + m_Parameters.rotation_matrix = Angle2RotationMat(res.x[0], res.x[1], res.x[2]); + m_Parameters.translation_vector = (cv::Mat_(3, 1) << res.x[3], res.x[4], res.x[5]); + m_Parameters.k1 = res.x[6]; + m_Parameters.k2 = res.x[7]; + m_Parameters.p1 = res.x[8]; + m_Parameters.p2 = res.x[9]; + m_Parameters.s1 = res.x[10]; + m_Parameters.s3 = res.x[11]; + + m_Parameters.image_spacing[0] = res.x[12]; + m_Parameters.image_spacing[1] = res.x[13]; + + m_Parameters.kx = m_Parameters.f / res.x[12]; + m_Parameters.ky = m_Parameters.f / res.x[13]; +} + +void TsaiCalibration::RefineFullParamters(const std::vector& points3D, const std::vector& points2D) +{ + auto obj = [=](ppx::MatrixS<16, 1> x) { + double _Rx = x[0]; + double _Ry = x[1]; + double _Rz = x[2]; + + double tx = x[3]; + double ty = x[4]; + double tz = x[5]; + + double k1 = x[6]; + double k2 = x[7]; + + double p1 = x[8]; + double p2 = x[9]; + + double s1 = x[10]; + double s3 = x[11]; + + double dx = x[12]; + double dy = x[13]; + + double u0 = x[14]; + double v0 = x[15]; + + double value = 0.0; + cv::Mat rm = Angle2RotationMat(_Rx, _Ry, _Rz); + for (size_t i = 0; i < points3D.size(); i++) + { + cv::Vec3f p = points3D[i]; + double zc = rm.at(2, 0) * p[0] + rm.at(2, 1) * p[1] + rm.at(2, 2) * p[2] + tz; + double xu = (rm.at(0, 0) * p[0] + rm.at(0, 1) * p[1] + rm.at(0, 2) * p[2] + tx) / zc; + double yu = (rm.at(1, 0) * p[0] + rm.at(1, 1) * p[1] + rm.at(1, 2) * p[2] + ty) / zc; + double rs = xu * xu + yu * yu; + + double x_pie = xu * (1 + k1 * rs + k2 * rs * rs) + 2 * p1 * xu * yu + p2 * (rs + 2 * xu * xu) + s1 * rs; + double y_pie = yu * (1 + k1 * rs + k2 * rs * rs) + p1 * (rs + 2 * yu * yu) + 2 * p2 * xu * yu + s3 * rs; + + double u = m_Parameters.f / dx * x_pie + u0; + double v = m_Parameters.f / dy * y_pie + v0; + + value += (u - points2D[i][0]) * (u - points2D[i][0]) + (v - points2D[i][1]) * (v - points2D[i][1]); + } + return value; + }; + + std::array angles = RotationMat2Angle(m_Parameters.rotation_matrix); + ppx::MatrixS<16, 1> x0 = { + angles[0], + angles[1], + angles[2], + m_Parameters.translation_vector.at(0,0), + m_Parameters.translation_vector.at(1,0), + m_Parameters.translation_vector.at(2,0), + m_Parameters.k1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + m_Parameters.image_spacing[0], + m_Parameters.image_spacing[1], + m_Parameters.u0, + m_Parameters.v0 + }; + auto res = ppx::fminunc(obj, x0); + std::cout << res.x.T() << std::endl; + + m_Parameters.rotation_matrix = Angle2RotationMat(res.x[0], res.x[1], res.x[2]); + m_Parameters.translation_vector = (cv::Mat_(3, 1) << res.x[3], res.x[4], res.x[5]); + m_Parameters.k1 = res.x[6]; + m_Parameters.k2 = res.x[7]; + m_Parameters.p1 = res.x[8]; + m_Parameters.p2 = res.x[9]; + m_Parameters.s1 = res.x[10]; + m_Parameters.s3 = res.x[11]; + + m_Parameters.image_spacing[0] = res.x[12]; + m_Parameters.image_spacing[1] = res.x[13]; + + m_Parameters.kx = m_Parameters.f / res.x[12]; + m_Parameters.ky = m_Parameters.f / res.x[13]; + + m_Parameters.u0 = res.x[14]; + m_Parameters.v0 = res.x[15]; +} + +/** + * @brief Non-linear optimization for external and distortion parameters + * @param points3D + * @param points2D +*/ +void TsaiCalibration::RefineExternalParameters(const std::vector& points3D, const std::vector& points2D) +{ + auto obj = [=](ppx::MatrixS<12, 1> x) { + double _Rx = x[0]; + double _Ry = x[1]; + double _Rz = x[2]; + + double tx = x[3]; + double ty = x[4]; + double tz = x[5]; + + double k1 = x[6]; + double k2 = x[7]; + + double p1 = x[8]; + double p2 = x[9]; + + double s1 = x[10]; + double s3 = x[11]; + + double value = 0.0; + cv::Mat rm = Angle2RotationMat(_Rx, _Ry, _Rz); + for (size_t i = 0; i < points3D.size(); i++) + { + cv::Vec3f p = points3D[i]; + double zc = rm.at(2, 0) * p[0] + rm.at(2, 1) * p[1] + rm.at(2, 2) * p[2] + tz; + double xu = (rm.at(0, 0) * p[0] + rm.at(0, 1) * p[1] + rm.at(0, 2) * p[2] + tx) / zc; + double yu = (rm.at(1, 0) * p[0] + rm.at(1, 1) * p[1] + rm.at(1, 2) * p[2] + ty) / zc; + double rs = xu * xu + yu * yu; + + double x_pie = xu * (1 + k1 * rs + k2 * rs * rs) + 2 * p1 * xu * yu + p2 * (rs + 2 * xu * xu) + s1 * rs; + double y_pie = yu * (1 + k1 * rs + k2 * rs * rs) + p1 * (rs + 2 * yu * yu) + 2 * p2 * xu * yu + s3 * rs; + + double u = m_Parameters.kx * x_pie + m_Parameters.u0; + double v = m_Parameters.ky * y_pie + m_Parameters.v0; + + value += (u - points2D[i][0]) * (u - points2D[i][0]) + (v - points2D[i][1]) * (v - points2D[i][1]); + } + return value; + }; + + std::array angles = RotationMat2Angle(m_Parameters.rotation_matrix); + ppx::MatrixS<12, 1> x0 = { + angles[0], + angles[1], + angles[2], + m_Parameters.translation_vector.at(0,0), + m_Parameters.translation_vector.at(1,0), + m_Parameters.translation_vector.at(2,0), + m_Parameters.k1, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0 + }; + auto res = ppx::fminunc(obj, x0); + std::cout << res.x.T() << std::endl; + + m_Parameters.rotation_matrix = Angle2RotationMat(res.x[0], res.x[1], res.x[2]); + m_Parameters.translation_vector = (cv::Mat_(3, 1) << res.x[3], res.x[4], res.x[5]); + m_Parameters.k1 = res.x[6]; + m_Parameters.k2 = res.x[7]; + m_Parameters.p1 = res.x[8]; + m_Parameters.p2 = res.x[9]; + m_Parameters.s1 = res.x[10]; + m_Parameters.s3 = res.x[11]; +} + + +void TsaiCalibration::RefineIntermalParameters(const std::vector& points3D, const std::vector& points2D) +{ + auto r1 = m_Parameters.rotation_matrix.rowRange(0, 1); + auto r2 = m_Parameters.rotation_matrix.rowRange(1, 2); + auto r3 = m_Parameters.rotation_matrix.rowRange(2, 3); + std::vector r1p, r2p, r3p; + std::transform(points3D.begin(), points3D.end(), std::back_inserter(r1p), + [=](auto p) {return r1.at(0, 0) * p[0] + r1.at(0, 1) * p[1] + r1.at(0, 2) * p[2]; }); + std::transform(points3D.begin(), points3D.end(), std::back_inserter(r2p), + [=](auto p) {return r2.at(0, 0) * p[0] + r2.at(0, 1) * p[1] + r2.at(0, 2) * p[2]; }); + std::transform(points3D.begin(), points3D.end(), std::back_inserter(r3p), + [=](auto p) {return r3.at(0, 0) * p[0] + r3.at(0, 1) * p[1] + r3.at(0, 2) * p[2]; }); + + + auto obj = [=](ppx::MatrixS<4, 1> x) { + double dx = x[0]; + double dy = x[1]; + double u0 = x[2]; + double v0 = x[3]; + + double value = 0.0; + + double tx = m_Parameters.translation_vector.at(0, 0); + double ty = m_Parameters.translation_vector.at(1, 0); + double tz = m_Parameters.translation_vector.at(2, 0); + + double k1 = m_Parameters.k1; + double k2 = m_Parameters.k2; + + double p1 = m_Parameters.p1; + double p2 = m_Parameters.p2; + + double s1 = m_Parameters.s1; + double s3 = m_Parameters.s3; + + for (size_t i = 0; i < points3D.size(); i++) + { + double xu = (r1p[i] + tx) / (r3p[i] + tz); + double yu = (r2p[i] + ty) / (r3p[i] + tz); + + double rs = xu * xu + yu * yu; + + double x_pie = xu * (1 + k1 * rs + k2 * rs * rs) + 2 * p1 * xu * yu + p2 * (rs + 2 * xu * xu) + s1 * rs; + double y_pie = yu * (1 + k1 * rs + k2 * rs * rs) + p1 * (rs + 2 * yu * yu) + 2 * p2 * xu * yu + s3 * rs; + + double u = m_Parameters.f / dx * x_pie + u0; + double v = m_Parameters.f / dy * y_pie + v0; + + value += hypot(u - points2D[i][0], v - points2D[i][1]); + } + return value; + }; + + ppx::MatrixS<4, 1> x0 = { + (double)m_Parameters.image_spacing[0], + (double)m_Parameters.image_spacing[1], + m_Parameters.u0, + m_Parameters.v0 + }; + auto res = ppx::fminunc(obj, x0); + std::cout << res.x.T() << std::endl; + + m_Parameters.image_spacing[0] = res.x[0]; + m_Parameters.image_spacing[1] = res.x[1]; + + m_Parameters.kx = m_Parameters.f / res.x[0]; + m_Parameters.ky = m_Parameters.f / res.x[1]; + + m_Parameters.u0 = res.x[2]; + m_Parameters.v0 = res.x[3]; +} + + +void TsaiCalibration::SetImageSize(const int& width, const int& height) +{ + m_Parameters.image_width = width; + m_Parameters.image_height = height; +} + + + +double TsaiCalibration::GetProjectionError() +{ + // undistortion the points by calibration + std::vector points2D_undistorted = GetUndistortedPoints(); + + // project 3d points with external parameter and calibrated camera matrix + std::vector projected_points2D_undistort = GetUndistortionProjectedPoints(); + + std::vector errors; + + //cv::Mat preview(1024, 1024, CV_8UC3); + for (size_t i = 0; i < points2D_undistorted.size(); i++) + { + //cv::circle(preview, cv::Point(projected_points2D_undistort[i]), 10, cv::Scalar(0, 0, 255), -1); + //cv::circle(preview, cv::Point(points2D_undistorted[i]), 10, cv::Scalar(255, 0, 255), 2); + errors.push_back(cv::norm(projected_points2D_undistort[i] - points2D_undistorted[i])); + } + //cv::imshow("preview2", preview); + //cv::waitKey(0); + + double maxe = *std::max_element(errors.begin(), errors.end()); + double mine = *std::min_element(errors.begin(), errors.end()); + double meane = std::reduce(errors.begin(), errors.end()) / errors.size(); + std::cout << "Max Projection Error = " << maxe << std::endl; + std::cout << "Min Projection Error = " << mine << std::endl; + std::cout << "Mean Projection Error = " << meane << std::endl; + + return meane; +} + +cv::Mat TsaiCalibration::UndistortImage(const cv::Mat& src) +{ + // distortion coeff + cv::Mat dist = (cv::Mat_(12, 1) << m_Parameters.k1, m_Parameters.k2, // k1,k2 + m_Parameters.p1, m_Parameters.p2, // p1,p2 + 0, 0, 0, 0, // k3,k4,k5,k6 + m_Parameters.s1, 0, m_Parameters.s3, 0); // s1,s2,s3,s4 + // camera matrix from calibration + cv::Mat camera_matrix = (cv::Mat_(3, 3) << m_Parameters.kx, 0, m_Parameters.u0, + 0, m_Parameters.ky, m_Parameters.v0, 0, 0, 1); + // standard camera matrix without optical center offset + cv::Mat camera_matrix_undistort = (cv::Mat_(3, 3) << m_Parameters.kx, 0, (m_Parameters.image_width - 1.0) / 2, + 0, m_Parameters.ky, (m_Parameters.image_height - 1.0) / 2, 0, 0, 1); + + cv::Size image_size(m_Parameters.image_width, m_Parameters.image_height); + cv::Mat dst; + cv::undistort(src, dst, camera_matrix, dist, camera_matrix); + //cv::Mat map1, map2; + //cv::initUndistortRectifyMap(camera_matrix, dist, cv::Mat(), camera_matrix, image_size, CV_32FC1, map1, map2); + //cv::remap(src, dst, map1, map2, cv::INTER_LINEAR); + return dst; +} + +/** + * @brief Get undistortion projected points from 3D points with calibrated external parameters + * and calibrated camera matrix. + * @return +*/ +std::vector TsaiCalibration::GetUndistortionProjectedPoints() +{ + // camera matrix from calibration + cv::Mat camera_matrix = (cv::Mat_(3, 3) << m_Parameters.kx, 0, m_Parameters.u0, + 0, m_Parameters.ky, m_Parameters.v0, 0, 0, 1); + + // project 3d points with external parameter but standard camera matrix + auto angles = RotationMat2Angle(m_Parameters.rotation_matrix); + cv::Mat rvec = (cv::Mat_(3, 1) << angles[0], angles[1], angles[2]); + + std::vector projected_points2D_undistort; + cv::projectPoints(m_Points3D, rvec, m_Parameters.translation_vector, // external parameter from calibration + camera_matrix, cv::Mat(), // project points in calibrated camera matrix + //camera_matrix_undistort, cv::Mat(), // project points in standard camera matrix (without optical center offset) + projected_points2D_undistort); + return projected_points2D_undistort; +} + + +/** + * @brief Get Undistorted points from distored points with calibrated external parameters + * and calibrated camera matrix. + * @return +*/ +std::vector TsaiCalibration::GetUndistortedPoints() +{ + // distortion coeff + cv::Mat dist = (cv::Mat_(12, 1) << m_Parameters.k1, m_Parameters.k2, // k1,k2 + m_Parameters.p1, m_Parameters.p2, // p1,p2 + 0, 0, 0, 0, // k3,k4,k5,k6 + m_Parameters.s1, 0, m_Parameters.s3, 0); // s1,s2,s3,s4 + // camera matrix from calibration + cv::Mat camera_matrix = (cv::Mat_(3, 3) << m_Parameters.kx, 0, m_Parameters.u0, + 0, m_Parameters.ky, m_Parameters.v0, 0, 0, 1); + // standard camera matrix without optical center offset + cv::Mat camera_matrix_undistort = (cv::Mat_(3, 3) << m_Parameters.kx, 0, (m_Parameters.image_width - 1.0) / 2, + 0, m_Parameters.ky, (m_Parameters.image_height - 1.0) / 2, 0, 0, 1); + // undistortion the points by calibration + std::vector points2D_undistorted; + cv::undistortPoints(m_Points2D, points2D_undistorted, + camera_matrix, dist, cv::Mat(), + camera_matrix); // undistort points with calibrated camera matrix + //camera_matrix_undistort); // undistort points with standard camera matrix (without optical center offset) + + return points2D_undistorted; +} \ No newline at end of file diff --git a/others/TsaiCpp/TsaiCalibration.h.xyzh b/others/TsaiCpp/TsaiCalibration.h.xyzh new file mode 100644 index 0000000..b14e100 --- /dev/null +++ b/others/TsaiCpp/TsaiCalibration.h.xyzh @@ -0,0 +1,129 @@ +#pragma once + +#include +#include + + +struct CameraParameters +{ + int image_width; + int image_height; + cv::Mat rotation_matrix; /* the rotation matrix from physical coordinate to camera coordinate. */ + cv::Mat translation_vector; /* the translation vector from physical coordinate to camera coordinate. */ + cv::Vec3d image_spacing; /* the spacing of x-ray image. */ + double k1; /*1st order radial distortion coeffient*/ + double k2; /*2nd order radial distortion coeffient*/ + double p1; /*1st order tangential distortion coeffient*/ + double p2; /*2nd order tangential distortion coeffient*/ + double s1; /*1st order thin prism distortion coeffient for x*/ + double s3; /*1st order thin prism distortion coeffient for y*/ + double f; /* the distance between x-ray source and detector.*/ + double kx; + double ky; + double u0; + double v0; + CameraParameters() : + k1(0), kx(0), ky(0), u0(0), v0(0), f(1011) {}; + + friend std::ostream& operator<<(std::ostream& os, const CameraParameters& para) + { + os << "Carm Transform Parameter: " << std::endl; + os << "\tf = " << para.f << std::endl; + os << "\tkx = " << para.kx << std::endl; + os << "\tky = " << para.ky << std::endl; + os << "\tTranslation Vector = " << para.translation_vector.t() << std::endl; + os << "\tImage Spacing = " << para.image_spacing << std::endl; + os << "\tsx = " << para.image_spacing[1] / para.image_spacing[0] << std::endl; + os << "\tu0 = " << para.u0 << std::endl; + os << "\tv0 = " << para.v0 << std::endl; + os << "\tdx = " << para.image_spacing[0] << std::endl; + os << "\tdy = " << para.image_spacing[1] << std::endl; + os << "\tk1 = " << para.k1 << std::endl; + os << "\tk2 = " << para.k2 << std::endl; + os << "\tp1 = " << para.p1 << std::endl; + os << "\tp2 = " << para.p2 << std::endl; + os << "\ts1 = " << para.s1 << std::endl; + os << "\ts3 = " << para.s3 << std::endl; + + + + return os; + } +}; + + + +/** + * @brief Linear and non-linear optimization for camera calibration by Tsai Step-2 method. + * This code is only support non-coplaner 3D points. + * all non-linear optimization follow the basic formula as below: + * $\begin{cases} + x_u=\frac{R_{11}X_w+R_{12}Y_w+R_{13}Z_w+T_x}{R_{31}X_w+R_{32}Y_w+R_{33}Z_w+T_z}, + y_u=\frac{R_{21}X_w+R_{22}Y_w+R_{23}Z_w+T_y}{R_{31}X_w+R_{32}Y_w+R_{33}Z_w+T_z}\\ + x^{''}=x_u(1+k_1r^2+k_2r^4)+2p_1x_uy_u+p_2(r^2+2x_u^2)+s_1r^2+s_2r^4\\ + y^{''}=y_u(1+k_1r^2+k_2r^4)+p_1(r^2+2y_u^2)+2p_2x_uy_u+s_3r^2+s_4r^4\\ + u \rightarrow \frac{f}{d_x}x^{''}+c_x,v \rightarrow \frac{f}{d_y}y^{''}+c_y\\ + ......\\ + J=\sqrt{||u-(\frac{f}{d_x}x^{''}+c_x)||^2+||v-(\frac{f}{d_y}y^{''}+c_y)||^2}\\ + \text{where: } r^2=x_u^2+y_u^2 + \end{cases}$ + where (Xw,Yw,Zw) notes the 3D points, and (u,v) notes the distorted image pixel. + * +*/ +class TsaiCalibration +{ +public: + + enum CalibrationType + { + ThreeStepOptimation=0, // + FiveStepOptimization + }; + + TsaiCalibration(); + ~TsaiCalibration(); + + void SetPoints3D(const std::vector& _arg) { m_Points3D = _arg; } + void SetPoints2D(const std::vector& _arg) { m_Points2D = _arg; } + + void SetInnerPoints3D(const std::vector& _arg) { m_InnerPoints3D = _arg; } + void SetInnerPoints2D(const std::vector& _arg) { m_InnerPoints2D = _arg; } + + void Setf(const double& _arg) { m_Parameters.f = _arg; } + + void SetImageSize(const int& width, const int& height); + + bool Execute(CalibrationType type); + + static cv::Mat Angle2RotationMat(const double& Rx, const double& Ry, const double& Rz); + static std::array RotationMat2Angle(const cv::Mat& rm); + + double GetProjectionError(); + CameraParameters GetCalibratedParameters() { return m_Parameters; } + std::vector GetUndistortionProjectedPoints(); + std::vector GetUndistortedPoints(); + cv::Mat UndistortImage(const cv::Mat& mat); + +private: + + bool ThreeStepCalibration(); + bool FiveStepCalibration(); + + bool Tsai(const std::vector& points3D, const std::vector& points2D); + void RefineTsai(const std::vector& points3D, const std::vector& points2D); + void RefineTsaiWithOpticalCenter(const std::vector& points3D, const std::vector& points2D); + void RefineExternalParameters(const std::vector& points3D, const std::vector& points2D); + void RefineIntermalParameters(const std::vector& points3D, const std::vector& points2D); + void RefineWithoutOpticalCenter(const std::vector& points3D, const std::vector& points2D); + void RefineFullParamters(const std::vector& points3D, const std::vector& points2D); + +private: + std::vector m_Points3D; + std::vector m_Points2D; + + // only valid for FiveStepMethod + std::vector m_InnerPoints3D; + std::vector m_InnerPoints2D; + + CameraParameters m_Parameters; +}; diff --git a/others/TsaiCpp/distort_image.png b/others/TsaiCpp/distort_image.png new file mode 100644 index 0000000..a3f4677 Binary files /dev/null and b/others/TsaiCpp/distort_image.png differ diff --git a/others/TsaiCpp/matrixs/exprtmpl.hpp b/others/TsaiCpp/matrixs/exprtmpl.hpp new file mode 100644 index 0000000..8ac2892 --- /dev/null +++ b/others/TsaiCpp/matrixs/exprtmpl.hpp @@ -0,0 +1,594 @@ +#ifndef VVERY_SIMPLE_EXPR_TMPL_HEADER +#define VVERY_SIMPLE_EXPR_TMPL_HEADER + +#include +#include +#include + +namespace ppx +{ + // constexpr + constexpr double PI = 3.141592653589793; + constexpr double EPS_SP = std::numeric_limits::epsilon(); + constexpr double EPS_DP = std::numeric_limits::epsilon(); + constexpr double MAX_SP = std::numeric_limits::max(); + constexpr double MAX_DP = std::numeric_limits::max(); + constexpr int MAJOR_VERSION = 1; + constexpr int MINOR_VERSION = 0; + + constexpr double DEG_RAD(double deg) + { + return deg * PI / 180; + } + + constexpr double RAD_DEG(double rad) + { + return 180 * rad / PI; + } + + template + using enable_when_array_t = std::enable_if_t; + template + using enable_when_matrix_t = std::enable_if_t; + + namespace details + { + // void_t + template + struct make_void + { + typedef void type; + }; + + template + using void_t = typename make_void::type; + + // operators + struct expr_plus_t + { + constexpr explicit expr_plus_t() = default; + + template + auto operator()(const LType &lhs, const RType &rhs) const + { + return lhs + rhs; + } + }; + + struct expr_minus_t + { + constexpr explicit expr_minus_t() = default; + + template + auto operator()(const LType &lhs, const RType &rhs) const + { + return lhs - rhs; + } + }; + + struct expr_mul_t + { + constexpr explicit expr_mul_t() = default; + + template + auto operator()(const LType &lhs, const RType &rhs) const + { + return lhs * rhs; + } + }; + + struct expr_div_t + { + constexpr explicit expr_div_t() = default; + + template + auto operator()(const LType &lhs, const RType &rhs) const + { + return lhs / rhs; + } + }; + + struct expr_abs_t + { + constexpr explicit expr_abs_t() = default; + + template + auto operator()(const Type &ele) const + { + return std::fabs(ele); + } + }; + + constexpr expr_plus_t expr_plus{}; + constexpr expr_minus_t expr_minus{}; + constexpr expr_mul_t expr_mul{}; + constexpr expr_div_t expr_div{}; + constexpr expr_abs_t expr_abs{}; + + struct ElemTags + { + struct Scalar; + struct Matrix; + struct Mblock; + }; + + // expr templates + template + struct scalar + { + private: + const T &s; + + public: + using elem_tag = ElemTags::Scalar; + + constexpr scalar(const T &v) : s(v) {} + + constexpr T const & + + operator[](std::size_t) const + { + return s; + } + + constexpr std::size_t + + size() const + { + return 0; + } + }; + + template + struct elem_traits + { + using elem_tag = Tag; + using elem_type = typename T::cast_type; + using elem_ref = T const &; + }; + + template + struct elem_traits, ElemTags::Scalar> + { + using elem_tag = ElemTags::Scalar; + using elem_type = T; + using elem_ref = scalar; + }; + + template + struct elem_traits + { + using elem_tag = ElemTags::Mblock; + using elem_type = typename T::cast_type; + using elem_ref = typename T::cast_type const &; + }; + + template + class expr + { + public: + const T &self() const + { + return static_cast(*this); + } + + T &self() + { + return static_cast(*this); + } + + auto eval() const + { + return (typename T::elem_type)self(); + } + + protected: + explicit expr() = default; + + constexpr size_t size() + { + return self().size_impl(); + } + + auto operator[](size_t idx) const + { + return self().at_impl(idx); + } + + auto operator()() const + { + return self()(); + } + }; + + template + class expr_elem : expr> + { + public: + using elem_tag = typename elem_traits::elem_tag; + using elem_type = typename elem_traits::elem_type; + using base_type = expr>; + using base_type::size; + using base_type::operator[]; + friend base_type; + + template ::value> * = nullptr> + explicit expr_elem(const T &val) : value(val) + { + } + template ::value> * = nullptr> + explicit expr_elem(const T &val) : value(val) + { + } + template ::value> * = nullptr> + explicit expr_elem(const T &val) : value(val.snap()) + { + } + + constexpr size_t size_impl() + { + return value.size(); + }; + + auto at_impl(size_t idx) const + { + return value[idx]; + }; + + decltype(auto) operator()() const + { + return (value); + } + + private: + typename elem_traits::elem_ref value; + }; + + template + class unops : public expr> + { + public: + using elem_tag = typename Expr::elem_tag; + using elem_type = typename Expr::elem_type; + + using base_type = expr>; + using base_type::size; + using base_type::operator[]; + friend base_type; + + explicit unops(const Ops &ops, const Expr &expr) + : m_ops(ops), m_expr(expr) {} + + constexpr size_t size_impl() + { + return m_expr.size(); + } + + auto at_impl(size_t idx) const + { + return m_ops(m_expr[idx]); + } + + template + operator T() const + { + T res{}; + for (size_t idx = 0; idx < res.size(); ++idx) + { + res[idx] = (*this)[idx]; + } + return res; + } + + private: + Ops m_ops; + Expr m_expr; + }; + + template + class biops : public expr> + { + public: + using elem_tag = + std::conditional_t::value, + typename rExpr::elem_tag, typename lExpr::elem_tag>; + using elem_type = + std::conditional_t::value, + typename rExpr::elem_type, typename lExpr::elem_type>; + using base_type = expr>; + using base_type::size; + using base_type::operator[]; + friend base_type; + + explicit biops(const Ops &ops, const lExpr &lxpr, const rExpr &rxpr) : m_ops(ops), m_lxpr(lxpr), m_rxpr(rxpr) {} + + constexpr size_t size_impl() + { + return std::max(m_lxpr.size(), m_rxpr.size()); + } + + auto at_impl(size_t idx) const + { + return m_ops(m_lxpr[idx], m_rxpr[idx]); + } + // dangerous but powerful; + template + operator T() const + { + T res{}; + for (size_t idx = 0; idx < res.size(); ++idx) + { + res[idx] = (*this)[idx]; + } + return res; + } + + private: + Ops m_ops; + lExpr m_lxpr; + rExpr m_rxpr; + }; + + template + struct is_matrix : public std::false_type + { + }; + + template + struct is_matrix> : public std::true_type + { + }; + + template + constexpr bool is_expr_v() + { + return std::is_base_of, T>::value; + } + + template + constexpr bool is_matrix_v() + { + return is_matrix::value; + } + + template + using enable_arith_type_t = std::enable_if_t::value, RT>; + + template + using enable_expr_type_t = std::enable_if_t(), RT>; + + template + using enable_matrix_type_t = std::enable_if_t(), RT>; + + template + using result_t = details::expr_elem; + + template + using result_s = details::expr_elem>; + + template + using enable_expr_expr_t = std::enable_if_t() && is_expr_v()>; + template + using enable_expr_num_t = std::enable_if_t() && std::is_arithmetic::value>; + template + using enable_expr_mat_t = std::enable_if_t() && is_matrix_v()>; + + template + using enable_num_expr_t = std::enable_if_t::value && is_expr_v()>; + template + using enable_num_mat_t = std::enable_if_t::value && is_matrix_v()>; + + template + using enable_mat_mat_t = std::enable_if_t() && is_matrix_v()>; + template + using enable_mat_expr_t = std::enable_if_t() && is_expr_v()>; + template + using enable_mat_num_t = std::enable_if_t() && std::is_arithmetic::value>; + + } // namespace details + + // Ops abs + template * = nullptr> + auto Abs(const T &t) + { + return details::unops(details::expr_abs, t); + } + + template * = nullptr> + auto Abs(const T &t) + { + return details::unops>(details::expr_abs, details::result_t(t)); + } + + // Ops + + template * = nullptr> + auto operator+(const T1 &t1, const T2 &t2) + { + return details::biops(details::expr_plus, t1, t2); + } + + template * = nullptr> + auto operator+(const T1 &t1, const T2 &t2) + { + return details::biops>(details::expr_plus, t1, details::result_s(t2)); + } + + template * = nullptr> + auto operator+(const T1 &t1, const T2 &t2) + { + return details::biops>(details::expr_plus, t1, details::result_t(t2)); + } + + template * = nullptr> + auto operator+(const T1 &t1, const T2 &t2) + { + return details::biops, T2>(details::expr_plus, details::result_s(t1), t2); + } + + template * = nullptr> + auto operator+(const T1 &t1, const T2 &t2) + { + return details::biops, + details::result_t>(details::expr_plus, details::result_s(t1), details::result_t(t2)); + } + + template * = nullptr> + auto operator+(const T1 &t1, const T2 &t2) + { + return details::biops, T2>(details::expr_plus, details::result_t(t1), t2); + } + + template * = nullptr> + auto operator+(const T1 &t1, const T2 &t2) + { + return details::biops, + details::result_s>(details::expr_plus, details::result_t(t1), details::result_s(t2)); + } + + template * = nullptr> + auto operator+(const T1 &t1, const T2 &t2) + { + return details::biops, + details::result_t>(details::expr_plus, details::result_t(t1), details::result_t(t2)); + } + + // Ops - + template * = nullptr> + auto operator-(const T1 &t1, const T2 &t2) + { + return details::biops(details::expr_minus, t1, t2); + } + + template * = nullptr> + auto operator-(const T1 &t1, const T2 &t2) + { + return details::biops>(details::expr_minus, t1, details::result_s(t2)); + } + + template * = nullptr> + auto operator-(const T1 &t1, const T2 &t2) + { + return details::biops>(details::expr_minus, t1, details::result_t(t2)); + } + + template * = nullptr> + auto operator-(const T1 &t1, const T2 &t2) + { + return details::biops, T2>(details::expr_minus, details::result_s(t1), t2); + } + + template * = nullptr> + auto operator-(const T1 &t1, const T2 &t2) + { + return details::biops, + details::result_t>(details::expr_minus, details::result_s(t1), details::result_t(t2)); + } + + template * = nullptr> + auto operator-(const T1 &t1, const T2 &t2) + { + return details::biops, T2>(details::expr_minus, details::result_t(t1), t2); + } + + template * = nullptr> + auto operator-(const T1 &t1, const T2 &t2) + { + return details::biops, + details::result_s>(details::expr_minus, details::result_t(t1), details::result_s(t2)); + } + + template * = nullptr> + auto operator-(const T1 &t1, const T2 &t2) + { + return details::biops, + details::result_t>(details::expr_minus, details::result_t(t1), details::result_t(t2)); + } + + // Ops * + template * = nullptr> + auto operator*(const T1 &t1, const T2 &t2) + { + return t1.eval() * t2.eval(); + } + + template * = nullptr> + auto operator*(const T1 &t1, const T2 &t2) + { + return details::biops>(details::expr_mul, t1, details::result_s(t2)); + } + + template * = nullptr> + auto operator*(const T1 &t1, const T2 &t2) + { + return t1.eval() * t2; + } + + template * = nullptr> + auto operator*(const T1 &t1, const T2 &t2) + { + return details::biops, T2>(details::expr_mul, details::result_s(t1), t2); + } + + template * = nullptr> + auto operator*(const T1 &t1, const T2 &t2) + { + return details::biops, + details::result_t>(details::expr_mul, details::result_s(t1), details::result_t(t2)); + } + + template * = nullptr> + auto operator*(const T1 &t1, const T2 &t2) + { + return details::biops, + details::result_s>(details::expr_mul, details::result_t(t1), details::result_s(t2)); + } + + template * = nullptr> + auto operator*(const T1 &t1, const T2 &t2) + { + return t1 * t2.eval(); + } + + // Ops / + template * = nullptr> + auto operator/(const T1 &t1, const T2 &t2) + { + return details::biops>(details::expr_div, t1, details::result_s(t2)); + } + + template * = nullptr> + auto operator/(const T1 &t1, const T2 &t2) + { + return details::biops, T2>(details::expr_div, details::result_s(t1), t2); + } + + template * = nullptr> + auto operator/(const T1 &t1, const T2 &t2) + { + return details::biops, + details::result_t>(details::expr_div, details::result_s(t1), details::result_t(t2)); + } + + template * = nullptr> + auto operator/(const T1 &t1, const T2 &t2) + { + return details::biops, + details::result_s>(details::expr_div, details::result_t(t1), details::result_s(t2)); + } + +} + +#endif \ No newline at end of file diff --git a/others/TsaiCpp/matrixs/liegroup.hpp b/others/TsaiCpp/matrixs/liegroup.hpp new file mode 100644 index 0000000..a06b78e --- /dev/null +++ b/others/TsaiCpp/matrixs/liegroup.hpp @@ -0,0 +1,219 @@ +#ifndef VVERY_SIMPLE_LIEGROUP_HEADER +#define VVERY_SIMPLE_LIEGROUP_HEADER + +#include "statistics.hpp" + +namespace ppx +{ + using so3 = MatrixS<3, 1>; + using se3 = MatrixS<6, 1>; + + inline MatrixS<3, 3> hat(const so3 &vec) + { + return {0.0, vec[2], -vec[1], -vec[2], 0.0, vec[0], vec[1], -vec[0], 0.0}; + } + + inline so3 vee(const MatrixS<3, 3> &mat) + { + return {mat[5], mat[6], mat[1]}; + } + + inline MatrixS<4, 4> hat(const se3 &vec) + { + return {0.0, vec[2], -vec[1], 0.0, -vec[2], 0.0, vec[0], 0.0, vec[1], -vec[0], 0.0, 0.0, vec[3], vec[4], vec[5], 0.0}; + } + + inline se3 vee(const MatrixS<4, 4> &mat) + { + return {mat[6], mat[8], mat[1], mat[12], mat[13], mat[14]}; + } + + class SO3 : public MatrixS<3, 3> + { + using Rep = MatrixS<3, 3>; + + public: + using MatrixS::MatrixS; + SO3(Rep &&other) : MatrixS(std::forward(other)) {} + SO3() : MatrixS(Rep::eye()){}; + SO3(const MatrixS<3, 1> &xAxis, const MatrixS<3, 1> &yAxis, const MatrixS<3, 1> &zAxis) + : MatrixS<3, 3>{xAxis[0], xAxis[1], xAxis[2], yAxis[0], yAxis[1], yAxis[2], zAxis[0], zAxis[1], zAxis[2]} {} + + SO3 operator*(const SO3 &other) const + { + return MatrixS::operator*(other); + } + template + MatrixS<3, L> operator*(const MatrixS<3, L> &other) const + { + return MatrixS::operator*(other); + } + MatrixS<3, 3> Adt() const + { + return *this; + } + SO3 I() const + { + return this->T(); + } + so3 log() const + { + const auto &R = *this; + auto acosinput = (this->trace() - 1) / 2.0; + if (acosinput >= 1.0) + { + return {}; + } + else if (acosinput <= -1.0) + { + MatrixS<3, 1> omg{}; + if (!details::near_zero(1 + R(2, 2))) + { + // omg = (1.0 / sqrt(2 + 2 * R(2, 2))) * MatrixS<3, 1>{R(0, 2), R(1, 2), R(2, 2)}; + omg = (1.0 / sqrt(2 + 2 * R(2, 2))) * R.col(2); + } + else if (!details::near_zero(1 + R(1, 1))) + { + omg = (1.0 / sqrt(2 + 2 * R(1, 1))) * R.col(1); + // omg = (1.0 / sqrt(2 + 2 * R(1, 1))) * MatrixS<3, 1>{R(0, 1), R(1, 1), +R(2, 1)}; + } + else + { + omg = (1.0 / sqrt(2 + 2 * R(0, 0))) * R.col(0); + // omg = (1.0 / sqrt(2 + 2 * R(0, 0))) * MatrixS<3, 1>{R(0, 0), R(1, 0), +R(2, 0)}; + } + return PI * omg; + } + else + { + MatrixS<3, 3> ret{}; + double theta = acos(acosinput); + ret = (R - R.T()) * theta / (2.0 * sin(theta)); + return vee(ret); + } + } + }; + + template <> + template <> + inline SO3 so3::exp<3, 1>() const + { + const auto &s = *this; + double theta = norm2(s); + if (details::near_zero(theta)) + { + return {}; + } + else + { + auto omg = hat(s); + auto coff1 = sin(theta) / theta; + auto coff2 = (1 - cos(theta)) / (theta * theta); + return SO3::eye() + coff1 * omg + coff2 * (omg * omg); + } + } + + template <> + template <> + inline MatrixS<3, 3> so3::adt<3, 1>() const + { + return hat(*this); + } + + class SE3 : public MatrixS<4, 4> + { + using Rep = MatrixS<4, 4>; + using T3 = MatrixS<3, 1>; + + public: + using MatrixS::MatrixS; + SE3(Rep &&other) : MatrixS(std::forward(other)) {} + SE3(const SO3 &Rot, const T3 &Pos) : MatrixS(Rep::eye()) + { + (*this).sub<3, 3, false>(0, 0) = Rot; + (*this).sub<3, 1, false>(0, 3) = Pos; + } + SE3() : MatrixS(Rep::eye()) {} + + SE3 operator*(const SE3 &other) const + { + return MatrixS::operator*(other); + } + template + MatrixS<4, L> operator*(const MatrixS<4, L> &other) const + { + return MatrixS::operator*(other); + } + SO3 Rot() const + { + return (*this).sub<3, 3>(0, 0); + } + T3 Pos() const + { + return (*this).sub<3, 1>(0, 3); + } + MatrixS<6, 6> Adt() const + { + MatrixS<6, 6> result{}; + const auto &R = Rot(); + const auto &p = Pos(); + result.sub<3, 3, false>(0, 0) = R; + result.sub<3, 3, false>(3, 0) = hat(p) * R; + result.sub<3, 3, false>(3, 3) = R; + return result; + } + SE3 I() const + { + return {Rot().T(), -1 * (Rot().T() * Pos())}; + } + se3 log() const + { + const auto &R = Rot(); + const auto &p = Pos(); + if (R == SO3{}) + { + return se3(T3{}, p); + } + else + { + auto theta = acos((R.trace() - 1) / 2.0); + auto omg = R.log(); + auto omgmat = hat(omg); + auto coff1 = (1.0 / theta - 1.0 / (tan(theta / 2.0) * 2.0)) / theta; + MatrixS<3, 3> logExpand = SO3::eye() - omgmat / 2.0 + coff1 * (omgmat * omgmat); + return se3(omg, logExpand * p); + } + } + }; + + template <> + template <> + inline SE3 se3::exp<6, 1>() const + { + auto phi = norm2(_1()); + if (details::near_zero(phi)) + { + return {SO3(), _2()}; + } + else + { + const auto ksi = hat(*this); + auto coff1 = (1 - cos(phi)) / (phi * phi); + auto coff2 = (phi - sin(phi)) / (phi * phi * phi); + return SE3::eye() + ksi + coff1 * (ksi * ksi) + coff2 * (ksi * ksi * ksi); + } + } + + template <> + template <> + inline MatrixS<6, 6> se3::adt<6, 1>() const + { + MatrixS<6, 6> result; + result.sub<3, 3, false>(0, 0) = hat(_1()); + result.sub<3, 3, false>(3, 0) = hat(_2()); + result.sub<3, 3, false>(3, 3) = hat(_1()); + return result; + } + +} +#endif \ No newline at end of file diff --git a/others/TsaiCpp/matrixs/linalg.hpp b/others/TsaiCpp/matrixs/linalg.hpp new file mode 100644 index 0000000..f182d51 --- /dev/null +++ b/others/TsaiCpp/matrixs/linalg.hpp @@ -0,0 +1,955 @@ +#ifndef VVERY_SIMPLE_ALGORITHM1_HEADER +#define VVERY_SIMPLE_ALGORITHM1_HEADER + +#include "matrixs.hpp" +#include + +namespace ppx +{ + inline double SIGN(double a, double b) + { + return b > 0.0 ? fabs(a) : -fabs(a); + } + + inline double SQR(double a) + { + return a * a; + } + + // matrix related + enum class Factorization : char + { + LU, + QR, + SVD + }; + + enum class EigenSystem : char + { + SymOnlyVal, + SymValAndVec, + SymValAndVecSorted, + // GemOnlyVal, + // GemValAndVec, + // GemValAndVecSorted + // todo + }; + + template + struct EigResult + { + MatrixS vec; + MatrixS val; + }; + + template + struct EqnResult + { + MatrixS x; + StatusCode s = StatusCode::NORMAL; + }; + + template + std::ostream &operator<<(std::ostream &os, const EqnResult &self) + { + os << "EqnResult<" << N << ">:\n" + << "Status:\t" << self.s << "\n" + << "x =\t" << self.x << std::endl; + return os; + } + + template + void zeros(MatrixS &m) + { + m.fill(0.0); + } + + template + void ones(MatrixS &m) + { + m.fill(1.0); + } + + template + MatrixS cofactor(const MatrixS &mat, size_t p, size_t q) + { + MatrixS result{}; + size_t i = 0, j = 0; + for (size_t row = 0; row < M; row++) + { + for (size_t col = 0; col < N; col++) + { + if (row != p && col != q) + { + result(i, j++) = mat(row, col); + if (j == N - 1) + { + j = 0; + i++; + } + } + } + } + return result; + } + + template + MatrixS adjugate(const MatrixS &mat) + { + MatrixS result{}; + for (size_t i = 0; i < M; i++) + { + for (size_t j = 0; j < M; j++) + { + auto sign = (i + j) % 2 == 0 ? 1 : -1; + result(j, i) = sign * (determinant(cofactor(mat, i, j))); + } + } + return result; + } + + template <> + inline MatrixS<1, 1> adjugate(const MatrixS<1, 1> &) + { + return {1}; + } + + template + double determinant(const MatrixS &mat) + { + return mat.det(); + } + + template + MatrixS inverse(const MatrixS &mat) + { + return mat.I(); + } + + template + MatrixS transpose(const MatrixS &m) + { + return m.T(); + } + + template + enable_when_array_t norm2(const MatrixS &mat) + { + double res = 0.0; + for (auto ele : mat) + { + res += ele * ele; + } + return sqrt(res); + } + + template + enable_when_array_t norminf(const MatrixS &mat) + { + double max = -std::numeric_limits::max(); + for (auto i : mat) + { + auto ti = fabs(i); + if (ti > max) + { + max = ti; + } + } + return max; + } + + template + double trace(const MatrixS &mat) + { + return mat.trace(); + } + + template + double inner_product(const MatrixS &a, const MatrixS &b) + { + return std::inner_product(a.cbegin(), a.cend(), b.cbegin(), 0.0); + } + + template + std::pair maxloc(const MatrixS &mat) + { + auto max_pos = std::max_element(mat.begin(), mat.end()); + auto max_dis = std::div(std::distance(mat.begin(), max_pos), M); + return {max_dis.rem, max_dis.quot}; + } + + // solver linear system + + template + FacResult ludcmp(MatrixS A, std::array &indx, bool &even) + { + constexpr int n = N; + even = true; + for (int i = 0; i < n; i++) + { + indx[i] = i; + } + for (int k = 0; k < n; k++) + { + auto valmax = fabs(A(k, k)); + auto ip = k; + for (int row = k + 1; row < n; row++) + { + double tmp = fabs(A(row, k)); + if (valmax < tmp) + { + valmax = tmp; + ip = row; + } + } + if (valmax < EPS_SP) + { + return {{}, StatusCode::SINGULAR}; + } + if (ip != k) + { + for (int col = k; col < n; col++) + { + std::swap(A(ip, col), A(k, col)); + } + std::swap(indx[ip], indx[k]); + for (int col = 0; col < k; col++) + { + std::swap(A(k, col), A(ip, col)); + } + even = !even; + } + for (int row = k + 1; row < n; row++) + { + double weight = A(row, k) / A(k, k); + A(row, k) = weight; + for (int col = k + 1; col < n; col++) + { + A(row, col) -= weight * A(k, col); + } + } + } + return {A, StatusCode::CONVERGED}; + } + + template + void ludbksb(const MatrixS &A, const std::array &indx, double *b) + { + constexpr int n = N; + std::array y{}; + y[0] = b[indx[0]]; + for (int row = 1; row < n; row++) + { + double sum = 0.0; + for (int col = 0; col < row; col++) + { + sum += A(row, col) * y[col]; + } + y[row] = b[indx[row]] - sum; + } + b[n - 1] = y[n - 1] / A(n - 1, n - 1); + for (int row = n - 2; row >= 0; row--) + { + auto id = row + 1; + double sum = 0.0; + for (int col = id; col < n; col++) + { + sum += A(row, col) * b[col]; + } + b[row] = (y[row] - sum) / A(row, row); + } + } + + template + FacResult qrdcmp(MatrixS a, MatrixS &c, MatrixS &d) + { + constexpr int m = M; + constexpr int n = N; + StatusCode sing = StatusCode::NORMAL; + double scale, sigma, sum, tau; + for (int k = 0; k < n; k++) + { + scale = 0.0; + for (int i = k; i < n; i++) + { + scale = std::max(scale, fabs(a(i, k))); + } + if (scale < EPS_SP) + { + sing = StatusCode::SINGULAR; + c[k] = 0.0; + d[k] = 0.0; + } + else + { + sum = 0.0; + for (int i = k; i < m; i++) + { + sum += a(i, k) * a(i, k); + } + sigma = fabs(a(k, k)) < EPS_SP ? sqrt(sum) : SIGN(sqrt(sum), a(k, k)); + a(k, k) += sigma; + c[k] = sigma * a(k, k); + // d[k] = -scale * sigma; + d[k] = -sigma; + for (int j = k + 1; j < n; j++) + { + sum = 0.0; + for (int i = k; i < m; i++) + { + sum += a(i, k) * a(i, j); + } + tau = sum / c[k]; + for (int i = k; i < m; i++) + { + a(i, j) -= tau * a(i, k); + } + // details::PRINT_SINGLE_ELEMENTS(a); + } + } + } + // d[N - 1] = a(N - 1, N - 1); + if (fabs(d[n - 1]) < EPS_SP) + { + sing = StatusCode::SINGULAR; + } + return {a, sing}; + } + + template + void qrsolv(const MatrixS &a, const MatrixS &c, const MatrixS &d, double *b) + { + constexpr int m = M; + constexpr int n = N; + for (int j = 0; j < n; j++) + { + double sum = 0.0; + for (int i = j; i < m; i++) + { + sum += a(i, j) * b[i]; + } + double tau = sum / c[j]; + for (int i = j; i < m; i++) + { + b[i] -= tau * a(i, j); + } + } + // b[N - 1] /= d[N - 1]; + for (int i = n - 1; i >= 0; i--) + { + double sum = 0.0; + for (int j = i + 1; j < n; j++) + { + sum += a(i, j) * b[j]; + } + b[i] = (b[i] - sum) / d[i]; + } + } + + template + FacResult svdcmp(MatrixS u, MatrixS &w, MatrixS &v) + { + constexpr int m = M; + constexpr int n = N; + int i, its, j, jj, k, nm; + double c, f, h, s, x, y, z; + std::array rv1{}; + double g = 0.0; + double scale = 0.0; + double anorm = 0.0; + int l = 0; + for (i = 0; i < n; i++) + { + l = i + 2; + rv1[i] = scale * g; + g = 0.0; + s = 0.0; + scale = 0.0; + if (i < m) + { + for (k = i; k < m; k++) + { + scale += fabs(u(k, i)); + } + if (scale > EPS_SP) + { + for (k = i; k < m; k++) + { + u(k, i) /= scale; + s += u(k, i) * u(k, i); + } + f = u(i, i); + g = -SIGN(sqrt(s), f); + h = f * g - s; + u(i, i) = f - g; + for (j = l - 1; j < n; j++) + { + for (s = 0.0, k = i; k < m; k++) + { + s += u(k, i) * u(k, j); + } + f = s / h; + for (k = i; k < m; k++) + { + u(k, j) += f * u(k, i); + } + } + for (k = i; k < m; k++) + { + u(k, i) *= scale; + } + } + } + w[i] = scale * g; + g = s = scale = 0.0; + if (i + 1 <= m && i + 1 != n) + { + for (k = l - 1; k < n; k++) + { + scale += fabs(u(i, k)); + } + if (scale > EPS_SP) + { + for (k = l - 1; k < n; k++) + { + u(i, k) /= scale; + s += u(i, k) * u(i, k); + } + f = u(i, l - 1); + g = -SIGN(sqrt(s), f); + h = f * g - s; + u(i, l - 1) = f - g; + for (k = l - 1; k < n; k++) + { + rv1[k] = u(i, k) / h; + } + for (j = l - 1; j < m; j++) + { + for (s = 0.0, k = l - 1; k < n; k++) + { + s += u(j, k) * u(i, k); + } + for (k = l - 1; k < n; k++) + { + u(j, k) += s * rv1[k]; + } + } + for (k = l - 1; k < n; k++) + { + u(i, k) *= scale; + } + } + } + anorm = std::max(anorm, fabs(w[i]) + fabs(rv1[i])); + } + for (i = n - 1; i >= 0; i--) + { + if (i < n - 1) + { + if (fabs(g) > EPS_SP) + { + for (j = l; j < n; j++) + { + v(j, i) = (u(i, j) / u(i, l)) / g; + } + for (j = l; j < n; j++) + { + for (s = 0.0, k = l; k < n; k++) + { + s += u(i, k) * v(k, j); + } + for (k = l; k < n; k++) + { + v(k, j) += s * v(k, i); + } + } + } + for (j = l; j < n; j++) + { + v(i, j) = 0.0; + v(j, i) = 0.0; + } + } + v(i, i) = 1.0; + g = rv1[i]; + l = i; + } + for (i = std::min(m, n) - 1; i >= 0; i--) + { + l = i + 1; + g = w[i]; + for (j = l; j < n; j++) + { + u(i, j) = 0.0; + } + if (fabs(g) > EPS_SP) + { + g = 1.0 / g; + for (j = l; j < n; j++) + { + for (s = 0.0, k = l; k < m; k++) + { + s += u(k, i) * u(k, j); + } + f = (s / u(i, i)) * g; + for (k = i; k < m; k++) + { + u(k, j) += f * u(k, i); + } + } + for (j = i; j < m; j++) + { + u(j, i) *= g; + } + } + else + { + for (j = i; j < m; j++) + { + u(j, i) = 0.0; + } + } + u(i, i) += 1.0; + } + for (k = n - 1; k >= 0; k--) + { + for (its = 0; its < 30; its++) + { + bool flag = true; + nm = k - 1; + for (l = k; l >= 0; l--) + { + nm = l - 1; + if (l == 0 || fabs(rv1[l]) <= EPS_SP * anorm) + { + flag = false; + break; + } + if (fabs(w[nm]) <= EPS_SP * anorm) + { + break; + } + } + if (flag) + { + c = 0.0; + s = 1.0; + for (i = l; i < k + 1; i++) + { + f = s * rv1[i]; + rv1[i] = c * rv1[i]; + if (fabs(f) <= EPS_SP * anorm) + { + break; + } + g = w[i]; + h = sqrt(f * f + g * g); + w[i] = h; + h = 1.0 / h; + c = g * h; + s = -f * h; + for (j = 0; j < m; j++) + { + y = u(j, nm); + z = u(j, i); + u(j, nm) = y * c + z * s; + u(j, i) = z * c - y * s; + } + } + } + z = w[k]; + if (l == k) + { + if (z < 0.0) + { + w[k] = -z; + for (j = 0; j < n; j++) + { + v(j, k) = -v(j, k); + } + } + break; + } + if (its == 29) + { + return {MatrixS(), StatusCode::SINGULAR}; + } + x = w[l]; + nm = k - 1; + y = w[nm]; + g = rv1[nm]; + h = rv1[k]; + f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y); + g = sqrt(f * f + 1.0); + f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x; + c = s = 1.0; + for (j = l; j <= nm; j++) + { + i = j + 1; + g = rv1[i]; + y = w[i]; + h = s * g; + g = c * g; + z = sqrt(f * f + h * h); + rv1[j] = z; + c = f / z; + s = h / z; + f = x * c + g * s; + g = g * c - x * s; + h = y * s; + y *= c; + for (jj = 0; jj < n; jj++) + { + x = v(jj, j); + z = v(jj, i); + v(jj, j) = x * c + z * s; + v(jj, i) = z * c - x * s; + } + z = sqrt(f * f + h * h); + w[j] = z; + if (fabs(z) > EPS_SP) + { + z = 1.0 / z; + c = f * z; + s = h * z; + } + f = c * g + s * y; + x = c * y - s * g; + for (jj = 0; jj < m; jj++) + { + y = u(jj, j); + z = u(jj, i); + u(jj, j) = y * c + z * s; + u(jj, i) = z * c - y * s; + } + } + rv1[l] = 0.0; + rv1[k] = f; + w[k] = x; + } + } + return {u, StatusCode::CONVERGED}; + } + + template + void svbksb(const MatrixS &u, const MatrixS &w, const MatrixS &v, double *b) + { + const int m = M; + const int n = N; + std::array tmp{}; + auto eigen_max = *std::max_element(w.cbegin(), w.cend()); + auto tsh = 0.5 * sqrt(m + n + 1) * eigen_max * EPS_SP; + for (int j = 0; j < n; j++) + { + auto s = 0.0; + if (w[j] > tsh) + { + for (int i = 0; i < m; i++) + { + s += u(i, j) * b[i]; + } + s /= w[j]; + } + tmp[j] = s; + } + for (int j = 0; j < n; j++) + { + auto s = 0.0; + for (int jj = 0; jj < n; jj++) + { + s += v(j, jj) * tmp[jj]; + } + b[j] = s; + } + } + + template + std::enable_if_t> + linsolve(const MatrixS &A, MatrixS b) + { + std::array indx{}; + auto even = true; + auto LU = ludcmp(A, indx, even); + if (LU.s == StatusCode::CONVERGED) + { + ludbksb(LU.x, indx, b.data()); + } + return {b, LU.s}; + } + + template + std::enable_if_t> + linsolve(const MatrixS &A, MatrixS b) + { + MatrixS c, d; + auto R = qrdcmp(A, c, d); + if (R.s != StatusCode::SINGULAR) + { + qrsolv(R.x, c, d, b.data()); + } + return {b.template sub(0, 0), R.s}; + } + + template + std::enable_if_t> + linsolve(const MatrixS &A, MatrixS b) + { + MatrixS w{}; + MatrixS V{}; + auto U = svdcmp(A, w, V); + if (U.s == StatusCode::CONVERGED) + { + svbksb(U.x, w, V, b.data()); + } + return {b.template sub(0, 0), U.s}; + } + + template + MatrixS pinv(const MatrixS &mat) + { + MatrixS w{}; + MatrixS V{}; + bool sing = false; + auto U = svdcmp(mat, w, V, sing); + if (sing) + { + return {}; + } + MatrixS W{}; + auto eigen_max = *std::max_element(w.cbegin(), w.cend()); + auto tsh = 0.5 * sqrt(M + N + 1) * eigen_max * EPS_SP; + for (size_t i = 0; i < N; i++) + { + if (fabs(w[i]) > tsh) + { + W(i, i) = 1.0 / w[i]; + } + } + return V * W * U.T(); + } + + // eigen system + namespace details + { + template + MatrixS tred2(MatrixS z, MatrixS &d, MatrixS &e) + { + constexpr int n = N; + for (int i = n - 1; i > 0; i--) + { + int l = i - 1; + double h = 0.0; + double scale = 0.0; + if (l > 0) + { + for (int k = 0; k < i; k++) + { + scale += fabs(z(i, k)); + } + if (fabs(scale) < EPS_SP) + { + e[i] = z(i, l); + } + else + { + for (int k = 0; k < i; k++) + { + z(i, k) /= scale; + h += z(i, k) * z(i, k); + } + double f = z(i, l); + double g = f > 0.0 ? -sqrt(h) : sqrt(h); + e[i] = scale * g; + h -= f * g; + z(i, l) = f - g; + f = 0.0; + for (int j = 0; j < i; j++) + { + z(j, i) = z(i, j) / h; + g = 0.0; + for (int k = 0; k < j + 1; k++) + { + g += z(j, k) * z(i, k); + } + for (int k = j + 1; k < i; k++) + { + g += z(k, j) * z(i, k); + } + e[j] = g / h; + f += e[j] * z(i, j); + } + double hh = f / (2 * h); + for (int j = 0; j < i; j++) + { + f = z(i, j); + e[j] = g = e[j] - hh * f; + for (int k = 0; k < j + 1; k++) + { + z(j, k) -= f * e[k] + g * z(i, k); + } + } + } + } + else + { + e[i] = z(i, l); + } + d[i] = h; + } + d[0] = 0.0; + e[0] = 0.0; + for (int i = 0; i < n; i++) + { + if (fabs(d[i]) > EPS_SP) + { + for (int j = 0; j < i; j++) + { + double g = 0.0; + for (int k = 0; k < i; k++) + { + g += z(i, k) * z(k, j); + } + for (int k = 0; k < i; k++) + { + z(k, j) -= g * z(k, i); + } + } + } + d[i] = z(i, i); + z(i, i) = 1.0; + for (int j = 0; j < i; j++) + { + z(j, i) = 0.0; + z(i, j) = 0.0; + } + } + return z; + } + + template + void tliq(MatrixS &z, MatrixS &d, MatrixS &e) + { + constexpr int n = N; + for (int i = 1; i < n; i++) + { + e[i - 1] = e[i]; + } + e[n - 1] = 0.0; + for (int l = 0; l < n; l++) + { + int iter = 0; + int m = 0; + do + { + for (m = l; m < n - 1; m++) + { + if (fabs(e[m]) < EPS_DP * (fabs(d[m]) + fabs(d[m + 1]))) + { + break; + } + } + if (m != l) + { + if (iter++ == 30) + // how ? throw("Too many iterations in tqli"); + { + return; + } + double g = (d[l + 1] - d[l]) / (2.0 * e[l]); + double r = sqrt(g * g + 1.0); + g = d[m] - d[l] + e[l] / (g + SIGN(r, g)); + double s = 1.0; + double c = 1.0; + double p = 0.0; + int i = 0; + for (i = m - 1; i >= l; i--) + { + double f = s * e[i]; + double b = c * e[i]; + r = sqrt(f * f + g * g); + e[i + 1] = r; + if (fabs(r) < EPS_SP) + { + d[i + 1] -= p; + e[m] = 0.0; + break; + } + s = f / r; + c = g / r; + g = d[i + 1] - p; + r = (d[i] - g) * s + 2.0 * c * b; + p = s * r; + d[i + 1] = g + p; + g = c * r - b; + for (int k = 0; k < n; k++) + { + f = z(k, i + 1); + z(k, i + 1) = s * z(k, i) + c * f; + z(k, i) = c * z(k, i) - s * f; + } + } + if (fabs(r) < EPS_SP && i >= l) + { + continue; + } + d[l] -= p; + e[l] = g; + e[m] = 0.0; + } + } while (m != l); + } + } + + template + void eigsrt(MatrixS &mat, MatrixS &vec) + { + constexpr int n = N; + for (auto i = 0; i < n - 1; i++) + { + auto j = std::distance(vec.begin() + i, std::min_element(vec.begin() + i, vec.end())) + i; + if (j != i) + { + std::swap(vec[i], vec[j]); + for (auto k = 0; k < n; k++) + { + std::swap(mat(k, i), mat(k, j)); + } + } + } + } + } + + template + std::enable_if_t> + eig(const MatrixS &mat) + { + MatrixS e, eig_value; + auto result = details::tred2(mat, eig_value, e); + details::tliq(result, eig_value, e); + return {result, eig_value}; + } + + template + std::enable_if_t> + eig(const MatrixS &mat) + { + MatrixS e, eig_value; + auto result = details::tred2(mat, eig_value, e); + details::tliq(result, eig_value, e); + details::eigsrt(result, eig_value); + return {result, eig_value}; + } + + template + std::enable_if_t> + eig(const MatrixS &mat) + { + MatrixS e, eig_value; + auto result = details::tred2(mat, eig_value, e); + details::tliq(result, eig_value, e); + std::sort(eig_value.begin(), eig_value.end()); + return eig_value; + } + +} +#endif \ No newline at end of file diff --git a/others/TsaiCpp/matrixs/matrixs.hpp b/others/TsaiCpp/matrixs/matrixs.hpp new file mode 100644 index 0000000..b50510e --- /dev/null +++ b/others/TsaiCpp/matrixs/matrixs.hpp @@ -0,0 +1,838 @@ +#ifndef VVERY_SIMPLE_MATRIXS_HEADER +#define VVERY_SIMPLE_MATRIXS_HEADER + +#include "exprtmpl.hpp" +#include +#include +#include +#include +#include +#include +#include + +namespace ppx +{ + // forward declare + template + class MatrixS; + + class SO3; + class SE3; + + enum class StatusCode : char + { + NORMAL, + CONVERGED, + DIVERGED, + SINGULAR + }; + + template + struct FacResult + { + MatrixS x; + StatusCode s = StatusCode::NORMAL; + }; + + inline std::ostream &operator<<(std::ostream &os, const StatusCode &self) + { + switch (self) + { + case StatusCode::NORMAL: + os << "NORMAL"; + break; + case StatusCode::CONVERGED: + os << "CONVERGED"; + break; + case StatusCode::DIVERGED: + os << "DIVERGED"; + break; + case StatusCode::SINGULAR: + os << "SINGULAR"; + default: + break; + } + return os; + } + + template + FacResult ludcmp(MatrixS A, MatrixS &indx, bool &even); + + template + FacResult svdcmp(MatrixS u, MatrixS &w, MatrixS &v); + + template + void ludbksb(const MatrixS &A, const MatrixS &indx, double *b); + + template + void svbksb(const MatrixS &u, const MatrixS &w, const MatrixS &v, double *b); + + namespace details + { + constexpr size_t MAX_SIZE_LIMIT = 260; + + inline bool is_same(double a, double b) + { + return fabs(a - b) < EPS_SP; + } + + inline bool near_zero(double a) + { + return fabs(a) < 1.0e-5; + } + + constexpr size_t is_small_matrix_v(size_t A, size_t B) + { + return A * B < MAX_SIZE_LIMIT ? 1 : 0; + } + + template + class MatrixBase; + + template + class MatrixBase + { + protected: + using iterator = typename std::array::iterator; + using const_iterator = typename std::array::const_iterator; + + MatrixBase() : m_data{} {} + + template * = nullptr> + explicit MatrixBase(const std::array &list) : m_data{} + { + constexpr auto real_idx = std::min(L, M * N); + std::copy_n(list.begin(), real_idx, m_data.begin()); + } + + template * = nullptr> + MatrixBase(const std::initializer_list &list) : m_data{} + { + auto real_idx = list.size() < M * N ? list.size() : M * N; + std::copy_n(list.begin(), real_idx, m_data.begin()); + } + + template * = nullptr> + explicit MatrixBase(const std::vector &list) : m_data{} + { + auto real_idx = list.size() < M * N ? list.size() : M * N; + std::copy_n(list.begin(), real_idx, m_data.begin()); + } + + protected: + std::array m_data; + }; + + template + class MatrixBase + { + protected: + using iterator = typename std::vector::iterator; + using const_iterator = typename std::vector::const_iterator; + + MatrixBase() : m_data(M * N, 0.0) {} + + template * = nullptr> + explicit MatrixBase(const std::array &list) : m_data(M * N, 0.0) + { + constexpr auto real_idx = std::min(L, M * N); + std::copy_n(list.begin(), real_idx, m_data.begin()); + } + + MatrixBase(const std::initializer_list &list) : m_data(M * N, 0.0) + { + auto real_idx = list.size() < M * N ? list.size() : M * N; + std::copy_n(list.begin(), real_idx, m_data.begin()); + } + + template * = nullptr> + explicit MatrixBase(const std::vector &list) : m_data(M * N, 0.0) + { + auto real_idx = list.size() < M * N ? list.size() : M * N; + std::copy_n(list.begin(), real_idx, m_data.begin()); + } + + protected: + std::vector m_data; + }; + + } // namespace details + + template + class MatrixS : public details::MatrixBase + { + template + using enable_when_squre_t = std::enable_if_t; + template + using disable_when_squre_t = std::enable_if_t; + + template + class SubMatrixBase + { + + public: + using elem_tag = details::ElemTags::Mblock; + using cast_type = MatrixS; + + SubMatrixBase(MatrixS &self, size_t r, size_t c) + : row_idx(r), col_idx(c), data(self) + { + assert(row_idx + A <= M && col_idx + B <= N); + } + + const cast_type &snap() const + { + return *copy; + } + + protected: + size_t row_idx; + size_t col_idx; + MatrixS &data; + std::unique_ptr> copy; + + void take_snap() + { + copy = std::make_unique>(); + for (size_t i = 0; i < A; i++) + { + for (size_t j = 0; j < B; j++) + { + (*copy)(i, j) = data(row_idx + i, col_idx + j); + } + } + } + + void copy_data(const SubMatrixBase &other) + { + for (size_t i = 0; i < A; i++) + { + for (size_t j = 0; j < B; j++) + { + data(row_idx + i, col_idx + j) = + other.data(other.row_idx + i, other.col_idx + j); + } + } + } + + template + void ctor_by_list(const T &list) + { + auto iter = list.begin(); + for (auto j = 0u; j < B; j++) + { + for (auto i = 0u; i < A; i++) + { + auto value = iter == list.end() ? typename T::value_type{} : *(iter++); + data(i + row_idx, j + col_idx) = value; + // which one is better? + // if (iter != list.end()) + // { + // data(i + row_idx, j + col_idx) = *(iter++); + // } + } + } + } + }; + + template + class SubMatrix : public SubMatrixBase + { + using base_type = SubMatrixBase; + + public: + using elem_tag = typename base_type::elem_tag; + using cast_type = typename base_type::cast_type; + + SubMatrix(MatrixS &self, size_t r, size_t c) + : SubMatrixBase(self, r, c) + { + SubMatrixBase::take_snap(); + } + + SubMatrix(const SubMatrix &other) + { + base_type::copy_data(other); + *base_type::copy = *other.copy; + } + + SubMatrix(SubMatrix &&other) noexcept : base_type::copy(std::move(other.copy)) + { + base_type::copy_data(other); + } + + SubMatrix &operator=(const SubMatrix &other) + { + base_type::copy_data(other); + *base_type::copy = *other.copy; + return *this; + } + + SubMatrix &operator=(SubMatrix &&other) noexcept + { + base_type::copy_data(other); + base_type::copy = std::move(other.copy); + return *this; + } + + SubMatrix &operator=(const MatrixS &other) + { + base_type::ctor_by_list(other); + return *this; + } + + template * = nullptr> + SubMatrix &operator=(const std::array &list) + { + base_type::ctor_by_list(list); + return *this; + } + + template * = nullptr> + SubMatrix &operator=(const std::initializer_list &list) + { + base_type::ctor_by_list(list); + return *this; + } + + template * = nullptr> + SubMatrix &operator=(const std::vector &list) + { + base_type::ctor_by_list(list); + return *this; + } + + template + details::enable_expr_type_t + operator=(const T &expr) + { + for (size_t j = 0; j < B; j++) + { + for (size_t i = 0; i < A; i++) + { + base_type::data(base_type::row_idx + i, base_type::col_idx + j) = expr[i + j * A]; + } + } + return *this; + } + + operator cast_type() + { + MatrixS result; + for (size_t i = 0; i < A; i++) + { + for (size_t j = 0; j < B; j++) + { + result(i, j) = base_type::data(base_type::row_idx + i, base_type::col_idx + j); + } + } + return result; + } + }; + + template + class SubMatrix : public SubMatrixBase + { + using base_type = SubMatrixBase; + + public: + using elem_tag = typename base_type::elem_tag; + using cast_type = typename base_type::cast_type; + + SubMatrix(MatrixS &self, size_t r, size_t c) + : SubMatrixBase(self, r, c) + { + } + + SubMatrix(const SubMatrix &other) + { + base_type::copy_data(other); + } + + SubMatrix(SubMatrix &&other) noexcept : base_type::copy(std::move(other.copy)) + { + base_type::copy_data(other); + } + + SubMatrix &operator=(const SubMatrix &other) + { + base_type::copy_data(other); + return *this; + } + + SubMatrix &operator=(SubMatrix &&other) noexcept + { + base_type::copy_data(other); + return *this; + } + + SubMatrix &operator=(const MatrixS &other) + { + base_type::ctor_by_list(other); + return *this; + } + + template * = nullptr> + SubMatrix &operator=(const std::array &list) + { + base_type::ctor_by_list(list); + return *this; + } + + template * = nullptr> + SubMatrix &operator=(const std::initializer_list &list) + { + base_type::ctor_by_list(list); + return *this; + } + + template * = nullptr> + SubMatrix &operator=(const std::vector &list) + { + base_type::ctor_by_list(list); + return *this; + } + + template + details::enable_expr_type_t + operator=(const T &expr) + { + for (size_t j = 0; j < B; j++) + { + for (size_t i = 0; i < A; i++) + { + base_type::data(base_type::row_idx + i, base_type::col_idx + j) = expr[i + j * A]; + } + } + return *this; + } + + operator cast_type() + { + MatrixS result; + for (size_t i = 0; i < A; i++) + { + for (size_t j = 0; j < B; j++) + { + result(i, j) = base_type::data(base_type::row_idx + i, base_type::col_idx + j); + } + } + return result; + } + }; + + public: + using value_type = double; + using cast_type = MatrixS; + using elem_tag = details::ElemTags::Matrix; + using iterator = typename details::MatrixBase::iterator; + using const_iterator = typename details::MatrixBase::const_iterator; + + iterator begin() noexcept + { + return this->m_data.begin(); + } + + iterator end() noexcept + { + return this->m_data.end(); + } + + const_iterator begin() const noexcept + { + return this->m_data.begin(); + } + + const_iterator end() const noexcept + { + return this->m_data.end(); + } + + const_iterator cbegin() const noexcept + { + return this->m_data.begin(); + } + + const_iterator cend() const noexcept + { + return this->m_data.end(); + } + + public: + MatrixS() = default; + + template * = nullptr> + explicit MatrixS(const std::array &list) : details::MatrixBase(list) + { + } + + template * = nullptr> + MatrixS(const std::initializer_list &list) : details::MatrixBase(list) + { + } + + template * = nullptr> + explicit MatrixS(const std::vector &list) : details::MatrixBase(list) + { + } + + template * = nullptr> + explicit MatrixS(const MatrixS<3, 1> &_1, const MatrixS<3, 1> &_2) + : details::MatrixBase({_1[0], _1[1], _1[2], _2[0], _2[1], _2[2]}) + { + } + + // Member functions + double *data() + { + return this->m_data.data(); + } + + const double *data() const + { + return this->m_data.data(); + } + + MatrixS row(size_t idx) const + { + assert(idx < M); + MatrixS result; + for (auto i = 0u; i < N; ++i) + { + result[i] = this->m_data.at(idx + i * M); + } + return result; + } + + MatrixS col(size_t idx) const + { + assert(idx < N); + MatrixS result; + for (auto i = 0u; i < M; ++i) + { + result[i] = this->m_data.at(idx * M + i); + } + return result; + } + + template + SubMatrix sub(size_t row_start, size_t col_start) + { + return {*this, row_start, col_start}; + } + + template + MatrixS sub(size_t row_start, size_t col_start) const + { + assert(row_start + A <= M && col_start + B <= N); + MatrixS result; + for (size_t i = 0; i < A; i++) + { + for (size_t j = 0; j < B; j++) + { + result(i, j) = (*this)(row_start + i, col_start + j); + } + } + return result; + } + + void fill(double val) + { + std::fill(this->m_data.begin(), this->m_data.end(), val); + } + + MatrixS T() const + { + MatrixS res{}; + for (auto i = 0u; i < M; i++) + { + for (auto j = 0u; j < N; j++) + { + res(j, i) = (*this)(i, j); + } + } + return res; + } + + template + enable_when_squre_t I() const + { + std::array indx{}; + auto even = true; + auto LU = ludcmp(*this, indx, even); + if (LU.s == StatusCode::SINGULAR) + { + return {}; + } + auto result = MatrixS::eye(); + for (size_t j = 0; j < M; j++) + { + ludbksb(LU.x, indx, result.data() + j * M); + } + return result; + } + + template + disable_when_squre_t> I() const + { + MatrixS w; + MatrixS V; + auto U = svdcmp(*this, w, V); + if (U.s == StatusCode::SINGULAR) + { + return {}; + } + MatrixS W; + for (size_t i = 0; i < N; i++) + { + if (fabs(w[i]) > EPS_SP) + { + W(i, i) = 1.0 / w[i]; + } + } + return V * W * U.x.T(); + } + + template + enable_when_squre_t det() const + { + auto even = true; + std::array indx{}; + auto LU = ludcmp(*this, indx, even); + if (LU.s == StatusCode::SINGULAR) + { + return {}; + } + auto D = even ? 1.0 : -1.0; + for (size_t i = 0; i < M; i++) + { + D *= LU.x(i, i); + } + return D; + } + + template + enable_when_squre_t trace() const + { + double res = 0.0; + for (size_t i = 0; i < A; i++) + { + res += (*this)(i, i); + } + return res; + } + + template + enable_when_matrix_t> diag() const + { + MatrixS result; + for (size_t i = 0; i < A; i++) + { + result[i] = (*this)(i, i); + } + return result; + } + + template + enable_when_array_t> diag() const + { + MatrixS result; + for (size_t i = 0; i < A; i++) + { + result(i, i) = (*this)[i]; + } + return result; + } + // Lie Group + template + std::enable_if_t exp() const; + + template + std::enable_if_t> adt() const; + + template + std::enable_if_t exp() const; + + template + std::enable_if_t> adt() const; + + template + std::enable_if_t> _1() const + { + return {(*this)[0], (*this)[1], (*this)[2]}; + } + + template + std::enable_if_t> _2() const + { + return {(*this)[3], (*this)[4], (*this)[5]}; + } + + // Overloaded Operators + double &operator()(size_t row, size_t col) + { + assert(row < M && col < N); + return this->m_data.at(row + col * M); + } + + const double &operator()(size_t row, size_t col) const + { + assert(row < M && col < N); + return this->m_data.at(row + col * M); + } + + double &operator()(const std::pair &idx) + { + assert(idx.first < M && idx.second < N); + return this->m_data.at(idx.first + idx.second * M); + } + + const double &operator()(const std::pair &idx) const + { + assert(idx.first < M && idx.second < N); + return this->m_data.at(idx.first + idx.second * M); + } + + double &operator[](size_t idx) + { + assert(idx < M * N); + return this->m_data.at(idx); + } + + const double &operator[](size_t idx) const + { + assert(idx < M * N); + return this->m_data.at(idx); + } + + template + MatrixS operator*(const MatrixS &other) const + { + MatrixS result; + for (size_t k = 0; k < N; k++) + { + for (size_t j = 0; j < L; j++) + { + for (size_t i = 0; i < M; i++) + { + result(i, j) += (*this)(i, k) * other(k, j); + } + } + } + return result; + } + + MatrixS &operator+=(const MatrixS &other) + { + for (size_t i = 0; i < M * N; i++) + { + this->m_data[i] += other.data()[i]; + } + return *this; + } + + MatrixS &operator-=(const MatrixS &other) + { + for (size_t i = 0; i < M * N; i++) + { + this->m_data[i] -= other.data()[i]; + } + return *this; + } + + bool operator==(const MatrixS &other) const + { + return std::equal(this->m_data.begin(), this->m_data.end(), other.data(), + [](double ele1, double ele2) + { return details::is_same(ele1, ele2); }); + } + + template + details::enable_arith_type_t operator+=(T ele) + { + for (auto &i : this->m_data) + { + i += ele; + } + return *this; + } + + template + details::enable_arith_type_t operator-=(T ele) + { + for (auto &i : this->m_data) + { + i -= ele; + } + return *this; + } + + template + details::enable_arith_type_t operator*=(T ele) + { + for (auto &i : this->m_data) + { + i *= ele; + } + return *this; + } + + template + details::enable_arith_type_t operator/=(T ele) + { + for (auto &i : this->m_data) + { + i /= ele; + } + return *this; + } + + // Generate function. + friend std::ostream &operator<<(std::ostream &os, const MatrixS &self) + { + os << "Matrix<" << M << "," << N << ">:\n"; + for (auto i = 0u; i < M; i++) + { + for (auto j = 0u; j < N; j++) + { + os << std::setw(12) << self(i, j) << "\t"; + } + os << std::endl; + } + return os; + } + + // Static function. + static MatrixS eye() + { + MatrixS result{}; + auto real_idx = M < N ? M : N; + for (size_t i = 0; i < real_idx; i++) + { + result.data()[i * (M + 1)] = 1.0; + } + return result; + } + + static MatrixS zeros() + { + return {}; + } + + constexpr size_t rows() const + { + return M; + } + + constexpr size_t cols() const + { + return N; + } + + constexpr size_t size() const + { + return M * N; + } + }; +} +#endif diff --git a/others/TsaiCpp/matrixs/optimization.hpp b/others/TsaiCpp/matrixs/optimization.hpp new file mode 100644 index 0000000..b80f598 --- /dev/null +++ b/others/TsaiCpp/matrixs/optimization.hpp @@ -0,0 +1,585 @@ +#ifndef VVERY_SIMPLE_ALGORITHM2_HEADER +#define VVERY_SIMPLE_ALGORITHM2_HEADER + +#include "linalg.hpp" + +namespace ppx +{ + enum class Optimization : char + { + GoldenSearch, + QuadraticSearch, + BrentSearch, + Powell, + GradientDescent, + ConjugateGradient, + BGFS + }; + + template + struct OptResult + { + MatrixS x; + double y = 0.0; + StatusCode s = StatusCode::NORMAL; + }; + + template <> + struct OptResult<1> + { + double x = 0.0; + double y = 0.0; + StatusCode s = StatusCode::NORMAL; + }; + + template + std::ostream &operator<<(std::ostream &os, const OptResult &self) + { + os << "OptResult<" << N << ">:\n" + << "Status:\t" << self.s << "\n" + << "xmin =\t" << self.x << "\n" + << "ymin =\t" << self.y << std::endl; + return os; + } + + namespace details + { + // 1D + struct Univariate + { + protected: + double a; + double b; + double c; + double ya; + double yb; + double yc; + + public: + void already_bounded(double x0, double x1) + { + a = std::min(x0, x1); + c = std::max(x0, x1); + b = 0.5 * (a + c); + } + + template + void bracket_minimum(const T &func, double x0) + { + // deal with monotony? + constexpr double k = 2.0; + constexpr size_t itmax = 1000; + double s = 0.05; + a = x0; + ya = func(a); + b = a + s; + yb = func(b); + if (yb > ya) + { + std::swap(a, b); + std::swap(ya, yb); + s = -s; + } + for (size_t its = 0; its < itmax; ++its) + { + c = b + s; + yc = func(c); + if (yc > yb) + { + if (a > c) + { + std::swap(a, c); + std::swap(ya, yc); + } + return; + } + a = b; + ya = yb; + b = c; + yb = yc; + s *= k; + } + } + }; + + struct GoldenSearch : Univariate + { + OptResult<1> R; + size_t ITMAX = 200; + + template + OptResult<1> operator()(const T &func) + { + constexpr double rho = 0.6180339887; + constexpr double phi = 1.0 - rho; + auto d = rho * c + phi * a; + auto yd = func(d); + auto its = 0u; + while (fabs(a - c) > EPS_SP && its < ITMAX) + { + R.x = rho * a + phi * c; + R.y = func(R.x); + if (R.y < yd) + { + c = d; + d = R.x; + yd = R.y; + } + else + { + a = c; + c = R.x; + } + ++its; + // printf("iter = %d, xc = %g, residual = %g\n", its, xmin, fabs(a - b)); + } + R.s = its == ITMAX ? StatusCode::DIVERGED : StatusCode::CONVERGED; + return R; + } + }; + + struct QuadraticSearch : Univariate + { + OptResult<1> R; + size_t ITMAX = 200; + + template + OptResult<1> operator()(const T &func) + { + ya = func(a); + yb = func(b); + yc = func(c); + auto its = 0u; + while (fabs(a - c) > EPS_SP && fabs(b - c) > EPS_SP && fabs(a - b) > EPS_SP && its < ITMAX) + { + R.x = 0.5 * (ya * (b * b - c * c) + yb * (c * c - a * a) + yc * (a * a - b * b)) / + (ya * (b - c) + yb * (c - a) + yc * (a - b)); + R.y = func(R.x); + if (R.x > b) + { + if (R.y > yb) + { + c = R.x; + yc = R.y; + } + else + { + a = b; + ya = yb; + b = R.x; + yb = R.y; + } + } + else + { + if (R.y > yb) + { + a = R.x; + ya = R.y; + } + else + { + c = b; + yc = yb; + b = R.x; + yb = R.y; + } + } + // printf("%g %g\n", xmin, ymin); + ++its; + } + R.s = its == ITMAX ? StatusCode::DIVERGED : StatusCode::CONVERGED; + return R; + } + }; + + struct BrentSearch : Univariate + { + OptResult<1> R; + size_t ITMAX = 200; + + template + OptResult<1> operator()(const T &func) + { + constexpr double rho = 0.6180339887; + constexpr double phi = 1.0 - rho; + auto x = c; + auto w = c; + auto v = c; + auto fx = func(x); + auto fw = fx; + auto fv = fx; + for (auto its = 0u; its < ITMAX; its++) + { + double d = 0.0; + double e = 0.0; + double u = 0.0; + auto xm = 0.5 * (a + c); + auto tol1 = EPS_SP * fabs(x); + auto tol2 = 2.0 * tol1; + if (fabs(x - xm) < tol2 - 0.5 * (c - a)) + { + R.y = fx; + R.x = x; + R.s = StatusCode::CONVERGED; + return R; + } + if (fabs(e) > tol1) + { + auto r = (x - w) * (fx - fv); + auto q = (x - v) * (fx - fw); + auto p = (x - v) * q - (x - w) * r; + q = 2.0 * (q - r); + if (q > 0.0) + { + p = -p; + } + q = fabs(q); + r = e; + e = d; + if (fabs(p) > fabs(0.5 * q * r) || + p < q * (a - x) || p > q * (c - x)) + { + e = x > xm ? a - x : c - x; + d = phi * e; + } + else + { + d = p / q; + u = x + d; + if (u - a < tol2 || c - u < tol2) + { + d = SIGN(tol1, xm - x); + } + } + } + else + { + e = x > xm ? a - x : c - x; + d = phi * e; + } + u = fabs(d) > tol1 ? x + d : x + SIGN(tol1, d); + auto fu = func(u); + if (fu < fx) + { + if (u > x) + { + a = x; + } + else + { + c = x; + } + v = w; + w = x; + x = u; + fv = fw; + fw = fx; + fx = fu; + } + else + { + if (u < x) + { + a = u; + } + else + { + c = u; + } + if (fu < fw || details::is_same(w, x)) + { + v = w; + w = u; + fv = fw; + fw = fu; + } + else if (fu < fv || details::is_same(v, x) || details::is_same(v, w)) + { + v = u; + fv = fu; + } + } + } + R.y = fx; + R.x = x; + R.s = StatusCode::DIVERGED; + return R; + } + }; + + template + OptResult<1> lnsrch(const T &f, const MatrixS &x, const MatrixS &d) + { + auto func = [&f, &x, &d](double a) + { + return f(x + a * d); + }; + BrentSearch brent; + brent.bracket_minimum(func, 0.0); + brent(func); + return brent.R; + } + + // nD + template + struct GradientDescent + { + OptResult R; + size_t ITMAX = 200; + double FTOLA = EPS_SP; + + template + OptResult operator()(const T1 &f, const T2 &df, const MatrixS &x) + { + R.x = x; + MatrixS dx; + dx.fill(1.0); + auto its = 0u; + while (norminf(dx) > FTOLA && its < ITMAX) + { + dx = -1 * df(R.x); + auto lr = lnsrch(f, R.x, dx); + if (lr.s == StatusCode::DIVERGED) + { + break; + } + dx *= lr.x; + R.x += dx; + R.y = lr.y; + // printf("iter = %d, xerr = %g\n", its++, norminf(dx)); + } + R.s = its == ITMAX ? StatusCode::DIVERGED : StatusCode::CONVERGED; + return R; + } + }; + + template + struct ConjuateGradient + { + OptResult R; + size_t ITMAX = 200; + double FTOLA = EPS_SP; + + template + OptResult operator()(const T1 &f, const T2 &df, const MatrixS &x) + { + R.x = x; + auto its = 0u; + MatrixS g = df(R.x); + MatrixS d = -1.0 * g; + MatrixS dh; + dh.fill(1.0); + while (norminf(dh) > FTOLA && its < ITMAX) + { + MatrixS gh = df(R.x); + double beta = + std::max(0.0, inner_product(gh, gh - g) / inner_product(g, g)); + dh = -1 * gh + beta * d; + auto lr = lnsrch(f, R.x, dh); + if (lr.s == StatusCode::DIVERGED) + { + break; + } + d = dh; + g = gh; + dh *= lr.x; + R.x += dh; + R.y = lr.y; + // printf("iter = %d, xerr = %g\n", its++, xerr); + } + R.s = its == ITMAX ? StatusCode::DIVERGED : StatusCode::CONVERGED; + return R; + } + }; + + template + struct BFGS + { + OptResult R; + size_t ITMAX = 20; + double FTOLA = EPS_SP; + + template + OptResult operator()(const T1 &f, const T2 &df, const MatrixS &x) + { + R.x = x; + auto Q = MatrixS::eye(); + MatrixS g = df(x); + MatrixS dg; + MatrixS dx; + dg.fill(1.0); + dx.fill(1.0); + auto its = 0u; + while (norm2(dx) > FTOLA && norm2(dg) > FTOLA && its < ITMAX) + { + dx = Q * g * (-1.0); + auto lr = lnsrch(f, R.x, dx); + if (lr.s == StatusCode::DIVERGED) + { + break; + } + dx *= lr.x; + R.x += dx; + R.y = lr.y; + auto gh = df(R.x); + dg = gh - g; + double nominator = inner_product(dx, dg); + auto dgT = dg.T(); + auto dxT = dx.T(); + double coff = (1 + dgT * Q * dg / nominator)[0] / nominator; + Q -= (dx * dgT * Q + Q * dg * dxT) / nominator + coff * (dx * dxT); + // Matrix tp = dx - Q * dg; + // Q += tp * tp.T() / inner_product(dg, tp); + g = gh; + // printf("iter = %d, xerr = %g\n", its++, norm2(dx)); + } + R.s = its == ITMAX ? StatusCode::DIVERGED : StatusCode::CONVERGED; + return R; + } + }; + + template + struct Powell + { + OptResult R; + size_t ITMAX = 200; + double FTOLA = EPS_SP; + + template + OptResult operator()(const T &f, const MatrixS &x0) + { + R.x = x0; + auto U = MatrixS::eye(); + auto dx = 1.0; + auto its = 0u; + while (dx > FTOLA && its < ITMAX) + { + auto x = R.x; + MatrixS d; + for (size_t i = 0; i < N; i++) + { + d = U.col(i); + auto lr = lnsrch(f, x, d); + // if (lr.s == StatusCode::DIVERGED) + // { + // R.s = StatusCode::DIVERGED; + // return; + // } + x += lr.x * d; + } + for (size_t i = 0; i < N - 1; i++) + { + + U.template sub(0, i) = U.template sub(0, i + 1); + // U({-1, -1}, i) = U.col(i + 1); + } + d = x - R.x; + U.template sub(0, N - 1) = d; + // U({-1, -1}, N - 1) = d; + auto lr = lnsrch(f, x, d); + if (lr.s == StatusCode::DIVERGED) + { + break; + } + x += lr.x * d; + dx = norm2(x - R.x); + R.x = x; + R.y = lr.y; + // printf("iters = %d, residual = %g\n", its++, dx); + } + R.s = its == ITMAX ? StatusCode::DIVERGED : StatusCode::CONVERGED; + return R; + } + }; + } + + template + std::enable_if_t> + fminbnd(const T &fn, double a, double b) + { + details::GoldenSearch gs; + gs.already_bounded(a, b); + return gs(fn); + } + + template + std::enable_if_t> + fminbnd(const T &fn, double a, double b) + { + details::QuadraticSearch qs; + qs.already_bounded(a, b); + return qs(fn); + } + + template + std::enable_if_t> + fminbnd(const T &fn, double a, double b) + { + details::BrentSearch brent; + brent.already_bounded(a, b); + return brent(fn); + } + + template + std::enable_if_t> + fminunc(const T &fn, double a) + { + details::GoldenSearch gs; + gs.bracket_minimum(fn, a); + return gs(fn); + } + + template + std::enable_if_t> + fminunc(const T &fn, double a) + { + details::QuadraticSearch qs; + qs.bracket_minimum(fn, a); + return qs(fn); + } + + template + std::enable_if_t> + fminunc(const T &fn, double a) + { + details::BrentSearch brent; + brent.bracket_minimum(fn, a); + return brent(fn); + } + + template + std::enable_if_t> + fminunc(const T &func, const MatrixS &x0) + { + details::Powell pw; + return pw(func, x0); + } + + template + std::enable_if_t> + fminunc(const T &func, const T2 &dfunc, const MatrixS &x0) + { + details::GradientDescent gd; + return gd(func, dfunc, x0); + } + + template + std::enable_if_t> + fminunc(const T &func, const T2 &dfunc, const MatrixS &x0) + { + details::ConjuateGradient cg; + return cg(func, dfunc, x0); + } + + template + std::enable_if_t> + fminunc(const T &func, const T2 &dfunc, const MatrixS &x0) + { + details::BFGS bfgs; + return bfgs(func, dfunc, x0); + } + +} + +#endif \ No newline at end of file diff --git a/others/TsaiCpp/matrixs/readme.txt b/others/TsaiCpp/matrixs/readme.txt new file mode 100644 index 0000000..acddb73 --- /dev/null +++ b/others/TsaiCpp/matrixs/readme.txt @@ -0,0 +1 @@ +https://github.com/Xtinc/matrix/tree/master/include \ No newline at end of file diff --git a/others/TsaiCpp/matrixs/statistics.hpp b/others/TsaiCpp/matrixs/statistics.hpp new file mode 100644 index 0000000..1435205 --- /dev/null +++ b/others/TsaiCpp/matrixs/statistics.hpp @@ -0,0 +1,284 @@ +#ifndef VVERY_SIMPLE_ALGORITHM3_HEADER +#define VVERY_SIMPLE_ALGORITHM3_HEADER + +#include "optimization.hpp" +#include + +namespace ppx +{ + // Moments of a Distribution: Mean, Variance, Skewness + template + auto mean(const T &vec) + { + using value_t = typename T::value_type; + return static_cast(std::accumulate(vec.begin(), vec.end(), value_t()) / vec.size()); + } + + template + double var(const T &vec) + { + using value_t = typename T::value_type; + value_t e = mean(vec); + double sum = 0.0; + for (size_t i = 0; i < vec.size(); i++) + { + value_t tmp = vec[i] - e; + sum += tmp * tmp; + } + return sum / (vec.size() - 1); + } + + template + class MultiNormalDistribution + { + public: + using samples = std::vector>; + MultiNormalDistribution() : m_cov(MatrixS::eye()), m_gen(std::random_device{}()) {} + MultiNormalDistribution(const MatrixS &mu, const MatrixS &sigma) + : m_mean(mu), m_cov(sigma) {} + MatrixS operator()() const + { + MatrixS x; + std::normal_distribution<> d{0, 1}; + auto eigsys = eig(m_cov); + MatrixS diag; + for (size_t i = 0; i < N; i++) + { + x[i] = d(m_gen); + diag(i, i) = sqrt(eigsys.val[i]); + } + return eigsys.vec * diag * x + m_mean; + } + + double pdf(const MatrixS &x) const + { + int n = static_cast(N); + MatrixS<2, 1> normalized_mu = x - m_mean; + double quadform = (normalized_mu.T() * m_cov.I() * normalized_mu)[0]; + double norm = pow(sqrt(2 * PI), -n) * pow(m_cov.det(), -0.5); + return norm * exp(-0.5 * quadform); + } + const MatrixS &mean() const + { + return m_mean; + } + MatrixS &mean() + { + return m_mean; + } + const MatrixS &covariance() const + { + return m_cov; + } + MatrixS &covariance() + { + return m_cov; + } + + void loglikehood(const samples &data) + { + auto n = data.size(); + auto sum_m = std::accumulate(data.begin(), data.end(), MatrixS()); + m_mean = sum_m / n; + MatrixS sum_s; + for (size_t i = 0; i < n; i++) + { + MatrixS tpx = data.at(i) - m_mean; + sum_s += tpx * tpx.T(); + } + m_cov = sum_s / n; + } + + private: + MatrixS m_mean; + MatrixS m_cov; + mutable std::mt19937 m_gen; + }; + + template + std::ostream &operator<<(std::ostream &os, const MultiNormalDistribution &self) + { + os << "MultiNormalDistribution<" << N << ">:\n" + << "mean = \t" << self.mean() + << "cov = \t" << self.covariance(); + return os; + } + + template + class MixedNormalDistribution + { + public: + using dist = MultiNormalDistribution; + using samples = std::vector>; + + MixedNormalDistribution() : m_prior(), m_gen(std::random_device{}()) {} + + double pdf(const MatrixS &x) + { + double sum = 0.0; + for (size_t i = 0; i < K; i++) + { + sum += m_guassian[i].pdf(x) * m_prior[i]; + } + return sum; + } + + MatrixS operator()() const + { + std::uniform_real_distribution<> dis(0.0, 1.0); + double sample = dis(m_gen); + double sum = m_prior.front(); + size_t idx = 0; + while (sample > sum && idx < K) + { + sum += m_prior[++idx]; + } + return m_guassian[idx](); + } + + const dist &distribution(size_t idx) const + { + return m_guassian.at(idx); + } + dist &distribution(size_t idx) + { + return m_guassian.at(idx); + } + const double &prior(size_t idx) const + { + return m_prior.at(idx); + } + double &prior(size_t idx) + { + return m_prior.at(idx); + } + void setcomp(size_t idx, const dist &d, double p) + { + m_guassian[idx] = d; + m_prior[idx] = p; + } + + void loglikehood(const samples &data) + { + constexpr auto c = K; + auto n = data.size(); + double residual = 1.0; + double last_p = 0.0; + auto its = 0u; + + while (residual > EPS_SP && its < ITMAX) + { + std::vector> a(c, std::vector(n)); + for (size_t k = 0; k < c; k++) + { + std::transform(data.begin(), data.end(), a[k].begin(), [&](const MatrixS &x) + { return m_prior[k] * m_guassian[k].pdf(x) / pdf(x); }); + } + for (size_t k = 0; k < c; k++) + { + auto sum_g = std::accumulate(a[k].begin(), a[k].end(), 0.0); + m_prior[k] = sum_g / n; + MatrixS sum_m = std::inner_product(a[k].begin(), a[k].end(), data.begin(), MatrixS()); + sum_m /= sum_g; + MatrixS sum_s; + for (size_t i = 0; i < n; i++) + { + MatrixS tpx = data.at(i) - sum_m; + sum_s += tpx * tpx.T() * a[k][i]; + } + m_guassian[k].mean() = sum_m; + m_guassian[k].covariance() = sum_s / sum_g; + } + double tpp = std::accumulate(data.begin(), data.end(), 0.0, [&](double y0, const MatrixS &x) + { return y0 + log(pdf(x)); }); + residual = fabs(last_p - tpp); + last_p = tpp; + ++its; + } + } + + private: + std::array m_guassian; + std::array m_prior; + mutable std::mt19937 m_gen; + size_t ITMAX = 200; + }; + + template + std::ostream &operator<<(std::ostream &os, const MixedNormalDistribution &self) + { + os << "MixedNormalDistribution<" << N << ',' << K << ">:\n"; + for (size_t i = 0; i < K; i++) + { + os << self.prior(i) << " of all, component " << i << ":\n"; + os << self.distribution(i) << "\n"; + } + return os; + } + + template + class Kmeans + { + public: + using samples = std::vector>; + explicit Kmeans(const samples &data) + : m_data(data), m_assign(m_data.size(), 0) {} + + private: + const samples &m_data; + std::vector m_assign; + std::array m_count; + std::array, K> m_mean; + + private: + int estep() + { + int nchg = 0; + int kmin = 0; + m_count.fill(0); + for (auto i = 0; i < m_data.size(); i++) + { + auto dmin = std::numeric_limits::max(); + for (auto j = 0; j < K; j++) + { + auto d = norm2(m_data[i] - m_mean[j]); + if (d < dmin) + { + dmin = d; + kmin = j; + } + } + if (kmin != m_assign[i]) + { + nchg++; + } + m_assign[i] = kmin; + m_count[kmin]++; + } + return nchg; + } + + void mstep() + { + for (const auto &elem : m_mean) + { + elem.fill(0); + } + + for (auto i = 0; i < m_data.size(); i++) + { + m_mean[m_assign[i]] += m_data[i]; + } + + for (auto i = 0; i < K; i++) + { + if (m_count[i] > 0) + { + m_mean[i] /= m_count[i]; + } + } + } + }; +} + +#endif \ No newline at end of file diff --git a/others/TsaiCpp/undistort_image_tsai3.png b/others/TsaiCpp/undistort_image_tsai3.png new file mode 100644 index 0000000..5605cd7 Binary files /dev/null and b/others/TsaiCpp/undistort_image_tsai3.png differ diff --git a/others/TsaiCpp/undistort_image_tsai5.png b/others/TsaiCpp/undistort_image_tsai5.png new file mode 100644 index 0000000..9d9771e Binary files /dev/null and b/others/TsaiCpp/undistort_image_tsai5.png differ diff --git a/others/marker3d.data b/others/marker3d.data new file mode 100644 index 0000000..7e29357 --- /dev/null +++ b/others/marker3d.data @@ -0,0 +1,76 @@ +{-40,80,0}, +{-20,80,0}, +{0,80,0}, +{20,80,0}, +{40,80,0}, +{-60,60,0}, +{-40,60,0}, +{-20,60,0}, +{0,60,0}, +{20,60,0}, +{40,60,0}, +{60,60,0}, +{-80,40,0}, +{-60,40,0}, +{-40,40,0}, +{-20,40,0}, +{0,40,0}, +{20,40,0}, +{40,40,0}, +{60,40,0}, +{80,40,0}, +{-80,20,0}, +{-60,20,0}, +{-40,20,0}, +{-20,20,0}, +{0,20,0}, +{20,20,0}, +{40,20,0}, +{60,20,0}, +{80,20,0}, +{-80,00,0}, +{-60,00,0}, +{-40,00,0}, +{-20,00,0}, +{0,00,0}, +{20,00,0}, +{40,00,0}, +{60,00,0}, +{80,00,0}, +{-80,-20,0}, +{-60,-20,0}, +{-40,-20,0}, +{-20,-20,0}, +{0,-20,0}, +{20,-20,0}, +{40,-20,0}, +{60,-20,0}, +{80,-20,0}, +{-80,-40,0}, +{-60,-40,0}, +{-40,-40,0}, +{-20,-40,0}, +{0,-40,0}, +{20,-40,0}, +{40,-40,0}, +{60,-40,0}, +{80,-40,0}, +{-60,-60,0}, +{-40,-60,0}, +{-20,-60,0}, +{0,-60,0}, +{20,-60,0}, +{40,-60,0}, +{60,-60,0}, +{-40,-80,0}, +{-20,-80,0}, +{0,-80,0}, +{20,-80,0}, +{40,-80,0}, +{30,10,0}, +{27.75,-9.25,-66}, +{27.75,-27.75,-66}, +{-9.25,-27.75,-66}, +{-30,-10,0}, +{-10,30,0}, +{9.25,27.75,-66} \ No newline at end of file diff --git a/others/points.data b/others/points.data new file mode 100644 index 0000000..666c6f1 --- /dev/null +++ b/others/points.data @@ -0,0 +1,76 @@ +{-40.000000,80.000000,0.000000,336.123535,905.153320}, +{-20.000000,80.000000,0.000000,432.812500,900.973633}, +{0.000000,80.000000,0.000000,528.764160,898.149963}, +{20.000000,80.000000,0.000000,623.738770,896.354248}, +{40.000000,80.000000,0.000000,720.455322,895.347656}, +{-60.000000,60.000000,0.000000,238.065247,810.660645}, +{-40.000000,60.000000,0.000000,335.826660,806.376709}, +{-20.000000,60.000000,0.000000,431.435791,803.155151}, +{0.000000,60.000000,0.000000,526.676758,800.280518}, +{20.000000,60.000000,0.000000,621.431885,797.779297}, +{40.000000,60.000000,0.000000,716.785156,796.461182}, +{60.000000,60.000000,0.000000,813.766113,796.046204}, +{-80.000000,40.000000,0.000000,138.096680,717.224976}, +{-60.000000,40.000000,0.000000,237.871094,713.796875}, +{-40.000000,40.000000,0.000000,334.535645,710.242432}, +{-20.000000,40.000000,0.000000,429.473389,707.514160}, +{0.000000,40.000000,0.000000,524.554199,704.752441}, +{20.000000,40.000000,0.000000,619.210815,702.304932}, +{40.000000,40.000000,0.000000,713.963867,700.302734}, +{60.000000,40.000000,0.000000,809.772461,698.799805}, +{80.000000,40.000000,0.000000,907.897461,698.220093}, +{-80.000000,20.000000,0.000000,137.220459,620.409180}, +{-60.000000,20.000000,0.000000,236.121704,617.883789}, +{-40.000000,20.000000,0.000000,332.453613,615.187622}, +{-20.000000,20.000000,0.000000,427.682617,612.872559}, +{0.000000,20.000000,0.000000,522.647461,610.322266}, +{20.000000,20.000000,0.000000,616.879395,607.560547}, +{40.000000,20.000000,0.000000,711.475830,605.208862}, +{60.000000,20.000000,0.000000,806.766602,603.061646}, +{80.000000,20.000000,0.000000,904.498047,601.563477}, +{-80.000000,0.000000,0.000000,134.722656,524.108032}, +{-60.000000,0.000000,0.000000,233.586426,522.379150}, +{-40.000000,0.000000,0.000000,329.943359,520.271240}, +{-20.000000,0.000000,0.000000,424.703613,517.704590}, +{0.000000,0.000000,0.000000,519.829102,515.601074}, +{20.000000,0.000000,0.000000,614.574219,513.280029}, +{40.000000,0.000000,0.000000,708.966309,510.420929}, +{60.000000,0.000000,0.000000,804.092041,508.076935}, +{80.000000,0.000000,0.000000,902.069946,505.772919}, +{-80.000000,-20.000000,0.000000,130.952148,427.737793}, +{-60.000000,-20.000000,0.000000,229.633789,426.819336}, +{-40.000000,-20.000000,0.000000,327.055725,425.150513}, +{-20.000000,-20.000000,0.000000,422.377930,423.075317}, +{0.000000,-20.000000,0.000000,517.172852,420.226074}, +{20.000000,-20.000000,0.000000,611.754395,417.883789}, +{40.000000,-20.000000,0.000000,706.426270,415.477539}, +{60.000000,-20.000000,0.000000,802.118164,412.938965}, +{80.000000,-20.000000,0.000000,900.335205,409.754395}, +{-80.000000,-40.000000,0.000000,126.254387,330.076843}, +{-60.000000,-40.000000,0.000000,226.290771,330.220093}, +{-40.000000,-40.000000,0.000000,323.598633,329.240723}, +{-20.000000,-40.000000,0.000000,418.720215,327.804688}, +{0.000000,-40.000000,0.000000,513.842285,325.474121}, +{20.000000,-40.000000,0.000000,609.453857,322.876953}, +{40.000000,-40.000000,0.000000,704.088989,320.213745}, +{60.000000,-40.000000,0.000000,800.612305,316.599609}, +{80.000000,-40.000000,0.000000,899.688965,312.547363}, +{-60.000000,-60.000000,0.000000,221.194702,231.743164}, +{-40.000000,-60.000000,0.000000,319.539062,231.923828}, +{-20.000000,-60.000000,0.000000,415.782715,230.721680}, +{0.000000,-60.000000,0.000000,511.454559,228.896484}, +{20.000000,-60.000000,0.000000,606.500977,226.162354}, +{40.000000,-60.000000,0.000000,702.766113,223.056122}, +{60.000000,-60.000000,0.000000,800.096436,219.090393}, +{-40.000000,-80.000000,0.000000,314.736328,131.224365}, +{-20.000000,-80.000000,0.000000,412.059235,130.770020}, +{0.000000,-80.000000,0.000000,508.189087,129.455078}, +{20.000000,-80.000000,0.000000,604.725098,126.927734}, +{40.000000,-80.000000,0.000000,701.933594,122.837891}, +{30.000000,10.000000,0.000000,663.097290,558.929199}, +{27.750000,-9.250000,-66.000000,658.681641,462.160034}, +{27.750000,-27.750000,-66.000000,655.680664,367.438965}, +{-9.250000,-27.750000,-66.000000,467.850098,372.420898}, +{-30.000000,-10.000000,0.000000,376.338623,471.643555}, +{-10.000000,30.000000,0.000000,476.395020,658.642578}, +{9.250000,27.750000,-66.000000,569.812988,652.845703} \ No newline at end of file diff --git a/others/points2.data b/others/points2.data new file mode 100644 index 0000000..479c485 --- /dev/null +++ b/others/points2.data @@ -0,0 +1,75 @@ +{-40.000000,80.000000,0.000000,330.400879,904.462402}, +{-20.000000,80.000000,0.000000,428.140137,901.526367}, +{0.000000,80.000000,0.000000,522.538086,899.511719}, +{20.000000,80.000000,0.000000,619.654297,899.097290}, +{40.000000,80.000000,0.000000,716.339355,899.449951}, +{-60.000000,60.000000,0.000000,234.660156,808.167480}, +{-40.000000,60.000000,0.000000,331.049072,806.463013}, +{-20.000000,60.000000,0.000000,427.512695,803.813965}, +{0.000000,60.000000,0.000000,522.418945,803.405029}, +{20.000000,60.000000,0.000000,616.833984,801.349609}, +{40.000000,60.000000,0.000000,713.443604,801.407959}, +{60.000000,60.000000,0.000000,810.607422,801.745605}, +{-80.000000,40.000000,0.000000,136.235596,713.412842}, +{-60.000000,40.000000,0.000000,234.340134,711.445496}, +{-40.000000,40.000000,0.000000,331.622559,709.408203}, +{-20.000000,40.000000,0.000000,427.509766,707.859863}, +{0.000000,40.000000,0.000000,521.870117,707.564453}, +{20.000000,40.000000,0.000000,616.780273,706.261719}, +{40.000000,40.000000,0.000000,711.654297,705.323242}, +{60.000000,40.000000,0.000000,807.978516,705.086731}, +{-80.000000,20.000000,0.000000,136.755859,616.091064}, +{-60.000000,20.000000,0.000000,234.842285,614.793457}, +{-40.000000,20.000000,0.000000,331.619629,613.635254}, +{-20.000000,20.000000,0.000000,426.454834,613.196045}, +{0.000000,20.000000,0.000000,521.637207,612.909668}, +{20.000000,20.000000,0.000000,616.126831,611.757812}, +{40.000000,20.000000,0.000000,710.775879,610.798828}, +{60.000000,20.000000,0.000000,806.259766,609.752441}, +{80.000000,20.000000,0.000000,904.436279,608.714844}, +{-80.000000,0.000000,0.000000,135.875488,519.897949}, +{-60.000000,0.000000,0.000000,234.728027,519.344727}, +{-40.000000,0.000000,0.000000,330.979492,518.814453}, +{-20.000000,0.000000,0.000000,425.386719,518.705566}, +{0.000000,0.000000,0.000000,521.203125,517.886230}, +{20.000000,0.000000,0.000000,615.551758,516.946777}, +{40.000000,0.000000,0.000000,710.098633,516.150635}, +{60.000000,0.000000,0.000000,811.257080,514.573730}, +{80.000000,0.000000,0.000000,903.019165,512.810547}, +{-80.000000,-20.000000,0.000000,133.672363,423.446533}, +{-60.000000,-20.000000,0.000000,232.699219,424.193848}, +{-40.000000,-20.000000,0.000000,329.609344,423.712891}, +{-20.000000,-20.000000,0.000000,425.335938,423.416504}, +{0.000000,-20.000000,0.000000,519.940918,422.835938}, +{20.000000,-20.000000,0.000000,614.643066,422.325439}, +{40.000000,-20.000000,0.000000,709.240112,421.090881}, +{60.000000,-20.000000,0.000000,804.807617,419.340332}, +{80.000000,-20.000000,0.000000,902.704102,416.572754}, +{-80.000000,-40.000000,0.000000,130.474609,326.383789}, +{-60.000000,-40.000000,0.000000,230.503906,327.609375}, +{-40.000000,-40.000000,0.000000,328.063354,328.076721}, +{-20.000000,-40.000000,0.000000,423.708496,327.661133}, +{0.000000,-40.000000,0.000000,518.425293,327.594238}, +{20.000000,-40.000000,0.000000,613.488770,326.662598}, +{40.000000,-40.000000,0.000000,708.560059,325.318848}, +{60.000000,-40.000000,0.000000,804.589844,322.728027}, +{80.000000,-40.000000,0.000000,903.556641,318.845703}, +{-60.000000,-60.000000,0.000000,226.739258,229.092163}, +{-40.000000,-60.000000,0.000000,324.797852,230.503418}, +{-20.000000,-60.000000,0.000000,421.780273,230.902832}, +{0.000000,-60.000000,0.000000,517.148560,230.736328}, +{20.000000,-60.000000,0.000000,612.469238,229.617188}, +{40.000000,-60.000000,0.000000,708.211548,227.622559}, +{60.000000,-60.000000,0.000000,805.263672,224.092468}, +{-40.000000,-80.000000,0.000000,320.934570,130.508301}, +{-20.000000,-80.000000,0.000000,418.652344,131.055969}, +{0.000000,-80.000000,0.000000,515.129639,130.969238}, +{20.000000,-80.000000,0.000000,611.170898,129.554688}, +{40.000000,-80.000000,0.000000,708.174438,126.900383}, +{30.000000,10.000000,0.000000,663.188354,563.931641}, +{27.750000,-9.250000,-66.000000,662.207642,466.744141}, +{27.750000,-27.750000,-66.000000,661.155396,372.210693}, +{-9.250000,-27.750000,-66.000000,473.103149,373.623047}, +{-30.000000,-10.000000,0.000000,377.761719,471.476807}, +{-10.000000,30.000000,0.000000,474.367920,660.234253}, +{9.250000,27.750000,-66.000000,569.320312,655.725098} \ No newline at end of file diff --git a/others/points_inner.data b/others/points_inner.data new file mode 100644 index 0000000..76a0c73 --- /dev/null +++ b/others/points_inner.data @@ -0,0 +1,16 @@ +{-20.000000,20.000000,0.000000,427.682617,612.872559}, +{0.000000,20.000000,0.000000,522.647461,610.322266}, +{20.000000,20.000000,0.000000,616.879395,607.560547}, +{-20.000000,0.000000,0.000000,424.703613,517.704590}, +{0.000000,0.000000,0.000000,519.829102,515.601074}, +{20.000000,0.000000,0.000000,614.574219,513.280029}, +{-20.000000,-20.000000,0.000000,422.377930,423.075317}, +{0.000000,-20.000000,0.000000,517.172852,420.226074}, +{20.000000,-20.000000,0.000000,611.754395,417.883789}, +{30.000000,10.000000,0.000000,663.097290,558.929199}, +{27.750000,-9.250000,-66.000000,658.681641,462.160034}, +{27.750000,-27.750000,-66.000000,655.680664,367.438965}, +{-9.250000,-27.750000,-66.000000,467.850098,372.420898}, +{-30.000000,-10.000000,0.000000,376.338623,471.643555}, +{-10.000000,30.000000,0.000000,476.395020,658.642578}, +{9.250000,27.750000,-66.000000,569.812988,652.845703} \ No newline at end of file diff --git a/others/pyTsai/.gitattributes b/others/pyTsai/.gitattributes new file mode 100644 index 0000000..412eeda --- /dev/null +++ b/others/pyTsai/.gitattributes @@ -0,0 +1,22 @@ +# Auto detect text files and perform LF normalization +* text=auto + +# Custom for Visual Studio +*.cs diff=csharp +*.sln merge=union +*.csproj merge=union +*.vbproj merge=union +*.fsproj merge=union +*.dbproj merge=union + +# Standard to msysgit +*.doc diff=astextplain +*.DOC diff=astextplain +*.docx diff=astextplain +*.DOCX diff=astextplain +*.dot diff=astextplain +*.DOT diff=astextplain +*.pdf diff=astextplain +*.PDF diff=astextplain +*.rtf diff=astextplain +*.RTF diff=astextplain diff --git a/others/pyTsai/.gitignore b/others/pyTsai/.gitignore new file mode 100644 index 0000000..5ebd21a --- /dev/null +++ b/others/pyTsai/.gitignore @@ -0,0 +1,163 @@ +################# +## Eclipse +################# + +*.pydevproject +.project +.metadata +bin/ +tmp/ +*.tmp +*.bak +*.swp +*~.nib +local.properties +.classpath +.settings/ +.loadpath + +# External tool builders +.externalToolBuilders/ + +# Locally stored "Eclipse launch configurations" +*.launch + +# CDT-specific +.cproject + +# PDT-specific +.buildpath + + +################# +## Visual Studio +################# + +## Ignore Visual Studio temporary files, build results, and +## files generated by popular Visual Studio add-ons. + +# User-specific files +*.suo +*.user +*.sln.docstates + +# Build results +[Dd]ebug/ +[Rr]elease/ +*_i.c +*_p.c +*.ilk +*.meta +*.obj +*.pch +*.pdb +*.pgc +*.pgd +*.rsp +*.sbr +*.tlb +*.tli +*.tlh +*.tmp +*.vspscc +.builds +*.dotCover + +## TODO: If you have NuGet Package Restore enabled, uncomment this +#packages/ + +# Visual C++ cache files +ipch/ +*.aps +*.ncb +*.opensdf +*.sdf + +# Visual Studio profiler +*.psess +*.vsp + +# ReSharper is a .NET coding add-in +_ReSharper* + +# Installshield output folder +[Ee]xpress + +# DocProject is a documentation generator add-in +DocProject/buildhelp/ +DocProject/Help/*.HxT +DocProject/Help/*.HxC +DocProject/Help/*.hhc +DocProject/Help/*.hhk +DocProject/Help/*.hhp +DocProject/Help/Html2 +DocProject/Help/html + +# Click-Once directory +publish + +# Others +[Bb]in +[Oo]bj +sql +TestResults +*.Cache +ClientBin +stylecop.* +~$* +*.dbmdl +Generated_Code #added for RIA/Silverlight projects + +# Backup & report files from converting an old project file to a newer +# Visual Studio version. Backup files are not needed, because we have git ;-) +_UpgradeReport_Files/ +Backup*/ +UpgradeLog*.XML + + + +############ +## Windows +############ + +# Windows image file caches +Thumbs.db + +# Folder config file +Desktop.ini + + +############# +## Python +############# + +*.py[co] + +# Packages +*.egg +*.egg-info +dist +build +eggs +parts +bin +var +sdist +develop-eggs +.installed.cfg + +# Installer logs +pip-log.txt + +# Unit test / coverage reports +.coverage +.tox + +#Translations +*.mo + +#Mr Developer +.mr.developer.cfg + +# Mac crap +.DS_Store diff --git a/others/pyTsai/AUTHORS b/others/pyTsai/AUTHORS new file mode 100644 index 0000000..f1a4c3e --- /dev/null +++ b/others/pyTsai/AUTHORS @@ -0,0 +1,26 @@ +============== +PyTsai Authors +============== + +Maintainer +========== + +Jonathan Merritt +Gabor Cseh + + +Original Tsai Calibration Code Developers +========================================= + +Frederic Devernay +Franz-Josef Luecke +Piotr Jasiobedzki +Markus Menke +Jon Owen +Pete Rander +Volker Rodehorst +Ron Steriti +Torfi Thorhallsson +Jim Vaughan +Reg Willson + diff --git a/others/pyTsai/BlenderTsai.py b/others/pyTsai/BlenderTsai.py new file mode 100644 index 0000000..248b7a2 --- /dev/null +++ b/others/pyTsai/BlenderTsai.py @@ -0,0 +1,784 @@ +#!BPY +# +# BlenderTsai.py +# Copyright (C) Jonathan Merritt 2004. +# +# This library is free software; you can redistribute it and/or +# modify it under the terms of the GNU Lesser General Public +# License as published by the Free Software Foundation; either +# version 2 of the License, or (at your option) any later version. +# +# This library is distributed in the hope that it will be useful, +# but WITHOUT ANY WARRANTY; without even the implied warranty of +# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU +# Library General Public License for more details. +# +# You should have received a copy of the GNU Lesser General Public +# License along with this library; if not, write to the +# Free Software Foundation, Inc., 59 Temple Place - Suite 330, +# Boston, MA 02111-1307, USA. +# + + + +################ +# IMPORT BLOCK # +################ + +import Blender, math +from Blender import BGL, Draw, Image, Object, Scene, Text, Window, Registry + +def menuError(str): + """ + Displays an error string in a pop-up menu. + """ + Draw.PupMenu('ERROR%%t|%s' % str) + +try: + import Tsai +except ImportError: + print 'This script requires the Tsai extension' + print 'module to be installed. This module could' + print 'not be imported. Please make sure that it' + print 'is installed.' + menuError('Could not import Tsai extension module.') + + + +#################### +# GLOBAL CONSTANTS # +#################### + +BUTTON_QUIT = 1 +BUTTON_LOAD = 2 +BUTTON_ZOOM = 3 +BUTTON_ADD = 4 +BUTTON_DELETE = 5 +BUTTON_FULLOPT = 6 +BUTTON_COPLANAR = 7 +BUTTON_CALIBRATE = 8 +BUTTON_OFSX = 9 +BUTTON_OFSY = 10 +BUTTON_OFSZ = 11 +ZOOM_MIN = 0.1 +ZOOM_MAX = 10.0 +ZOOM_DELTA = 0.075 +OFS_MIN = -100.0 +OFS_MAX = 100.0 +MODE_NORMAL = 1 +MODE_ADD = 2 +MODE_GRAB = 3 +MAX_SELECT_DIST = 30 +SCRIPTLINK_NAME = '__TsaiCC_REDRAW' + +# These colors should later be set by the Theme module +# once it is implemented. +COLOR_BACKGROUND = (130/255.0, 130/255.0, 130/255.0, 1.0) +COLOR_CROSSHAIRS = (1.0, 0.5, 0.5, 0.4) +COLOR_MINIMAP = (0.0, 0.0, 0.0, 1.0) +COLOR_VERTSEL = (255/255.0, 255/255.0, 112/255.0, 1.0) +COLOR_VERTUNSEL = (255/255.0, 112/255.0, 255/255.0, 1.0) +COLOR_TEXT = (1.0, 1.0, 1.0, 1.0) +POINT_SIZE = 4 + + +#################### +# GLOBAL VARIABLES # +#################### + +class ADict: + pass +G = ADict() +G.buttons = ADict() +G.buttons.quit = None +G.buttons.load = None +G.buttons.zoom = None +G.buttons.fullopt = None +G.buttons.coplanar = None +G.buttons.ofsx = None +G.buttons.ofsy = None +G.buttons.ofsz = None +G.buttons.calibrate = None +G.buttons.add = None +G.buttons.delete = None +G.zoom = 1.0 +G.fullopt = 0 +G.coplanar = 1 +G.ofsx = 0.0 +G.ofsy = 0.0 +G.ofsz = 0.0 +G.imagename = None +G.image = None +#G.ibuf = None +G.iw = 0 +G.ih = 0 +G.imgpos = [10,10] +G.mousepos = None +G.grabpos = None +G.mode = MODE_NORMAL +G.selection = [] +G.last_camera = None +G.curempty = None +# coordmap is a map between the names of Blender +# empties and 2D image coordinates (stored as +# tuples). +G.coordmap = {} + + + +########### +# METHODS # +########### + + +def addCustomScriptLink(): + + """ + Adds a custom script link to the scene so that all script + windows are redrawn whenever the scene is redrawn. + """ + + txt = Text.New(SCRIPTLINK_NAME) + txt.write('import Blender\n') + txt.write('from Blender import Window\n') + txt.write('Window.Redraw(Window.Types.SCRIPT)\n') + Scene.GetCurrent().addScriptLink(SCRIPTLINK_NAME, 'Redraw') + + # do a full scene update + Scene.GetCurrent().update(1) + + +def removeCustomScriptLink(): + + """ + Removes the custom script link (if it is present) from the + scene. + """ + + try: + txt = Text.Get(SCRIPTLINK_NAME) + except NameError: # script link not found + return + + scene = Scene.GetCurrent() + slinks = scene.getScriptLinks('Redraw') + if slinks is not None: + scene.clearScriptLinks() + if SCRIPTLINK_NAME in slinks: + slinks.remove(SCRIPTLINK_NAME) + for link in slinks: + scene.addScriptLink(link, 'Redraw') + + Text.unlink(txt) + + # do a full scene update + Scene.GetCurrent().update(1) + + +def getWinRect(): + + """ + Returns the rectangle of the current script window in + screen coordinates. + + @return: Script window rectangle: [x, y, w, h] + """ + + winrect = BGL.Buffer(BGL.GL_FLOAT, 4) + BGL.glGetFloatv(BGL.GL_SCISSOR_BOX, winrect) + return winrect.list + + +def getWMCoords(): + + """ + Returns the coordinates of the mouse relative to the + current script window: (x,y) + + @return: Current script window mouse coordinates: (x,y). + """ + + xm,ym = Window.GetMouseCoords() + xw,yw,ww,hw = getWinRect() + return (xm-xw, ym-yw) + + +def wc2ic(p): + + """ + Converts window coordinates to image coordinates. + + @param p: Window coordinates (x,y). + + @return: Image coordinates (x,y). + """ + + xi = p[0] / G.zoom - int(G.imgpos[0] / G.zoom) + yi = p[1] / G.zoom - int(G.imgpos[1] / G.zoom) + if xi < 0: xi = 0 + if yi < 0: yi = 0 + if G.image is not None: + if xi > G.iw: xi = G.iw + if yi > G.ih: yi = G.ih + return (xi, yi) + + +def setZoom(p, zoom): + + """ + Sets the zoom factor, G.zoom. This method maintains the + centre of zoom (p) at the same location before and after + the zoom. + + @param p: Centre of the zoom (xw,yw), in window coordinates. + @param zoom: New zoom factor. + """ + + # bounds-check the zoom factor + if zoom < ZOOM_MIN: zoom = ZOOM_MIN + if zoom > ZOOM_MAX: zoom = ZOOM_MAX + + # if we have no image then just set the zoom + global G + if G.image is None: + G.zoom = zoom + Draw.Redraw(1) + return + + # find centre of zoom in image coordinates + xi = (p[0] - G.imgpos[0]) / G.zoom + yi = (p[1] - G.imgpos[1]) / G.zoom + + # set the new zoom + G.zoom = zoom + + # find the new centre of zoom + xinew = (p[0] - G.imgpos[0]) / G.zoom + yinew = (p[1] - G.imgpos[1]) / G.zoom + + # find the offset for the origin + xofs, yofs = int(xinew - xi), int(yinew - yi) + + # change the origin + G.imgpos[0] += xofs * G.zoom + G.imgpos[1] += yofs * G.zoom + + # queue a redraw + Draw.Redraw(1) + + +def calibrate(camera = None): + + """ + Performs calibration. + """ + + # TODO: Add warnings about insufficient numbers of points + + if camera == None: + camera = G.selection[0] + + if G.coplanar: target_type = 'coplanar' + else: target_type = 'noncoplanar' + + if G.fullopt: optimization_type = 'full' + else: optimization_type = 'three-param' + + origin_offset = (G.ofsx, G.ofsy, G.ofsz) + + calcoords = [] + for (emptyname, imgcoords) in G.coordmap.items(): + m = Object.Get(emptyname).getMatrix('worldspace') + xs = m[3][0] + ys = m[3][1] + zs = m[3][2] + xi = imgcoords[0] + yi = G.ih - imgcoords[1] # note the change to the y-axis there! + #print xs, ys, zs, xi, yi + calcoords.append((xs, ys, zs, xi, yi)) + + cp = Tsai.CameraParameters(image_dim=(G.iw, G.ih)) + try: + cp = Tsai.calibrate(target_type, optimization_type, calcoords, cp, origin_offset) + except Tsai.CalibrationError, msg: + menuError(msg) + return + + cp.setBlenderCamera(camera, G.iw, G.ih) + + Window.RedrawAll() + + +def loadTGARAW(fileName): + + """ + Loads a RAW TGA image from a file. On success, the method + returns: (image, imagebuffer). On failure, the method + returns None. + + @param filename: Name of the RAW TGA file to load. + + @return: (image, imagebuffer) 2-tuple on success, or None + on failure. + """ + + # create the image and allocate the image buffer using + # Blender's own methods + image = Image.Load(fileName) + #ibuf = BGL.Buffer(BGL.GL_BYTE, (image.size[1], image.size[0], 4)) + + ## read in the file + #infile = open(fileName, 'rb') + #array = map(ord, infile.read()) + #infile.close() + + ## check the validity of the file and offset the data + ## array to the start of the image data + #if array[2] is not 2: + # menuError('Can only load RAW TGA images.') + # return None + #array = array[(18+array[0]):] + + # read the array into the image buffer + #index = 0 + #for y in range(0, image.size[1]): + # for x in range(0, image.size[0]): + # ibuf[y][x][2] = array[index] + # ibuf[y][x][1] = array[index+1] + # ibuf[y][x][0] = array[index+2] + # index += 3 + + # return the (image, imagebuffer) tuple + return (image,) + + +def loadImage(fileName): + + """ + Handles loading an image. This method populates + G.image, G.ibuf, G.iw, G.ih. + + @param fileName: Name of the file to load. + """ + + Window.WaitCursor(True) + + # load the image and set up parameters + global G + try: + iloaded = loadTGARAW(fileName) + except Exception: + menuError('Could not load image.') + iloaded = None + if iloaded is None: + return + (G.image,) = iloaded + G.iw, G.ih = G.image.size + G.imagename = fileName + + # centre the image in the window rectangle + xw,yw,ww,hw = getWinRect() + G.imgpos[0] = (ww-G.iw) / 2 + G.imgpos[1] = (hw-G.ih) / 2 + + Window.WaitCursor(False) + + + + + +def removeUnknownsFromCoords(): + + """ + Checks that all names of empties in G.coordmap + actually exist in the scene. If they don't, they're + pruned from the map. + """ + + global G + + for emptyname in G.coordmap.keys(): + try: + Object.Get(emptyname) + except AttributeError: + del G.coordmap[emptyname] + + +def writeDataToRegistry(): + + """ + Writes data to the Blender registry. + """ + + global G + + dict = {} + dict['coordmap'] = G.coordmap + if G.imagename is not None: + dict['imagename'] = G.imagename + + Registry.SetKey('TsaiCC', dict) + + +def readDataFromRegistry(): + + """ + Reads data from the Blender registry. + """ + + global G + + dict = Registry.GetKey('TsaiCC') + if dict: + G.coordmap = dict['coordmap'] + removeUnknownsFromCoords() + # TODO: + try: + loadImage(dict['imagename']) + except KeyError: + pass + + +def renderGUI(): + + """ + Renders the GUI for the script. + """ + + global G + + # find the selection set and update some selection + # related flags + haveEmpty = False + haveCamera = False + emptyName = "" + G.selection = Object.GetSelected() + G.curempty = None + if G.selection is not None and len(G.selection) > 0: + mso = G.selection[0] + msotype = mso.getType() + if msotype == 'Empty': + haveEmpty = True + G.curempty = mso + emptyName = G.curempty.getName() + elif msotype == 'Camera': + haveCamera = True + emptyHasCoords = G.coordmap.has_key(emptyName) + removeUnknownsFromCoords() + + # clear any buttons that need to have set states + G.buttons.add = G.buttons.delete = None + + # clear the window + c = COLOR_BACKGROUND + BGL.glClearColor(c[0], c[1], c[2], c[3]) + BGL.glClear(BGL.GL_COLOR_BUFFER_BIT) + + # paint the image in the background + if G.image is not None: + #drawImage(G.image, G.imgpos, G.iw, G.ih, G.zoom) + Draw.Image(G.image, G.imgpos[0], G.imgpos[1], G.zoom, G.zoom) + # paint 2D vertices in the image + BGL.glPushAttrib(BGL.GL_POINT_BIT | BGL.GL_CURRENT_BIT) + BGL.glPointSize(POINT_SIZE) + x0 = int(G.imgpos[0] / G.zoom) + y0 = int(G.imgpos[1] / G.zoom) + def drawvc(ec): + emptyname, coord = ec + if Object.Get(emptyname) in G.selection: + c = COLOR_VERTSEL + else: + c = COLOR_VERTUNSEL + BGL.glColor4f(c[0], c[1], c[2], c[3]) + BGL.glVertex2f(G.zoom*(coord[0]+x0), G.zoom*(coord[1]+y0)) + BGL.glBegin(BGL.GL_POINTS) + map(drawvc, G.coordmap.items()) + BGL.glEnd() + BGL.glPopAttrib() + + # if we're in add mode then draw some extra stuff + if G.mode == MODE_ADD: + + xm,ym = map(int, getWMCoords()) + xm -= 10 + ym += 10 + (xw,yw,ww,hw) = getWinRect() + + # draw crosshairs + c = COLOR_CROSSHAIRS + BGL.glColor4f(c[0], c[1], c[2], c[3]) + verts = [ (xm,0), (xm,hw), (0,ym), (ww,ym) ] + BGL.glBegin(BGL.GL_LINES) + map(lambda x: BGL.glVertex2d(x[0],x[1]), verts) + BGL.glEnd() + + ############################################# + # UNCOMMENT THIS SECTION FOR A COOL MINIMAP + # EFFECT - NOT VERY USEFUL THOUGH + # + ## draw "minimap" background + #c = COLOR_MINIMAP + #BGL.glColor4f(c[0], c[1], c[2], c[3]) + #verts = [ (119,10), (221,10), (221,111), (119,111) ] + #BGL.glBegin(BGL.GL_QUADS) + #map(lambda x: BGL.glVertex2i(x[0],x[1]), verts) + #BGL.glEnd() + # + ## paint the image into the minimap + #ix,iy = wc2ic((xm,ym)) + #ix,iy = map(int, [ix, iy]) + #drawImage(G.ibuf, (120,10), G.iw, G.ih, 10.0, (ix-5,iy-5,10,10)) + # + # END OF MINIMAP SECTION + ############################################# + + # paint the current empty name + if haveEmpty: + c = COLOR_TEXT + BGL.glColor4d(c[0], c[1], c[2], c[3]) + BGL.glRasterPos2i(220, 10) + Draw.Text(emptyName, 'small') + + # paint the normal GUI buttons + G.buttons.quit = Draw.PushButton('Quit', BUTTON_QUIT, 5, 5, 100, 20, 'Exits the script.') + G.buttons.load = Draw.PushButton('Load Image', BUTTON_LOAD, 5, 25, 100, 20, 'Loads an image.') + G.buttons.zoom = Draw.Number('Zoom', BUTTON_ZOOM, 5, 45, 100, 20, G.zoom, ZOOM_MIN, ZOOM_MAX, 'Adjusts image zoom.') + + # paint camera-specific stuff + if haveCamera: + G.buttons.fullopt = Draw.Toggle('Full Optimization', BUTTON_FULLOPT, 110, 5, 120, 20, G.fullopt, 'Full or partial optimization.') + G.buttons.coplanar = Draw.Toggle('Coplanar', BUTTON_COPLANAR, 110, 25, 120, 20, G.coplanar, 'Coplanar or non-coplanar target.') + # Origin offset is not currently working in the Tsai module. + # It should be brought back here when it is. + #G.buttons.ofsz = Draw.Number('OfsZ', BUTTON_OFSZ, 110, 50, 100, 20, G.ofsz, OFS_MIN, OFS_MAX, 'Z origin offset.') + #G.buttons.ofsy = Draw.Number('OfsY', BUTTON_OFSY, 110, 70, 100, 20, G.ofsy, OFS_MIN, OFS_MAX, 'Y origin offset.') + #G.buttons.ofsx = Draw.Number('OfsX', BUTTON_OFSX, 110, 90, 100, 20, G.ofsx, OFS_MIN, OFS_MAX, 'X origin offset.') + G.buttons.calibrate = Draw.PushButton('Calibrate', BUTTON_CALIBRATE, 235, 5, 100, 20, 'Calibrates the selected camera.') + + # paint empty-specific stuff + elif haveEmpty and (G.mode == MODE_NORMAL): + if emptyHasCoords: + G.buttons.delete = Draw.PushButton('Delete', BUTTON_DELETE, 110, 5, 100, 20, 'Adds an image calibration coordinate.') + else: + G.buttons.add = Draw.PushButton('Add', BUTTON_ADD, 110, 5, 100, 20, 'Removes an image calibration coordinate.') + + +def eventHandler(event, value): + + """ + General GUI event handler. + + @param event: Event type. + @param value: Value of the event. + """ + + global G + + if event == Draw.WHEELDOWNMOUSE: + setZoom(getWMCoords(), G.zoom*(1.0-ZOOM_DELTA)) + + elif event == Draw.WHEELUPMOUSE: + setZoom(getWMCoords(), G.zoom*(1.0+ZOOM_DELTA)) + + elif event == Draw.MIDDLEMOUSE: + if value: G.mousepos = Window.GetMouseCoords() + else: G.mousepos = None + + elif event == Draw.MOUSEX or event == Draw.MOUSEY: + + mouseButs = Window.GetMouseButtons() + + if (mouseButs & Draw.MIDDLEMOUSE) and (G.mousepos is not None): + nx,ny = Window.GetMouseCoords() + dx,dy = nx-G.mousepos[0], ny-G.mousepos[1] + G.mousepos = (nx,ny) + G.imgpos = [int(G.imgpos[0]+dx), int(G.imgpos[1]+dy)] + Draw.Redraw(1) + + elif G.mode == MODE_ADD: + Draw.Redraw(1) + + elif G.mode == MODE_GRAB: + G.havebupclik = True + nx,ny = Window.GetMouseCoords() + dx,dy = (nx-G.grabpos[0])/G.zoom, (ny-G.grabpos[1])/G.zoom + G.grabpos = [nx,ny] + def translate(x): + name = x.getName() + if G.coordmap.has_key(name): + c = G.coordmap[name] + G.coordmap[name] = (c[0]+dx, c[1]+dy) + map(translate, G.selection) + ## autocalibration.. gets stuck some times.. + #if G.last_camera: + # calibrate(G.last_camera) + Draw.Redraw(1) + + elif (event == Draw.LEFTMOUSE) or (event == Draw.RETKEY): + + if (G.mode == MODE_ADD) and (value == 1): + x,y = map(int, getWMCoords()) + x -= 10 + y += 10 + G.coordmap[G.curempty.getName()] = wc2ic((x,y)) + G.mode = MODE_NORMAL + Draw.Redraw(1) + + elif (G.mode == MODE_GRAB) and (value == 1): + G.mode = MODE_NORMAL + Draw.Redraw(1) + + elif (event == Draw.RIGHTMOUSE) and (value == 1): + + if G.mode == MODE_NORMAL: + xi,yi = wc2ic(getWMCoords()) + closest = None + for (emptyname, coord) in G.coordmap.items(): + dist = math.sqrt((coord[0]-xi)**2 + (coord[1]-yi)**2) * G.zoom + if (closest == None) or (dist < closest[0]): + closest = (dist, emptyname) + if closest[0] < MAX_SELECT_DIST: + obj = Object.Get(closest[1]) + kq = Window.GetKeyQualifiers() + if (kq & Window.Qual.LSHIFT) or (kq & Window.Qual.RSHIFT): + obj.select(True) + else: + map(lambda x: x.select(False), G.selection) + obj.select(True) + Window.RedrawAll() + + elif (event == Draw.AKEY) and (value == 1): + + if G.mode == MODE_NORMAL: + someSelected = False + for (emptyname, coord) in G.coordmap.items(): + if Object.Get(emptyname).isSelected(): + someSelected = True + break + newselect = (someSelected == False) + map(lambda x: Object.Get(x[0]).select(newselect), G.coordmap.items()) + Window.RedrawAll() + + elif (event == Draw.GKEY) and (value == 1): + + if G.mode == MODE_NORMAL: + G.mode = MODE_GRAB + G.grabpos = Window.GetMouseCoords() + Draw.Redraw(1) + + elif event == Draw.UPARROWKEY and value == 1: + + p = Window.GetMouseCoords() + Window.SetMouseCoords(p[0], p[1]+1) + + elif event == Draw.DOWNARROWKEY and value == 1: + + p = Window.GetMouseCoords() + Window.SetMouseCoords(p[0], p[1]-1) + + elif event == Draw.LEFTARROWKEY and value == 1: + + p = Window.GetMouseCoords() + Window.SetMouseCoords(p[0]-1, p[1]) + + elif event == Draw.RIGHTARROWKEY and value == 1: + + p = Window.GetMouseCoords() + Window.SetMouseCoords(p[0]+1, p[1]) + + elif event == Draw.XKEY and value == 1: + + if len(G.selection) > 0: + result = Draw.PupMenu('OK?%t|Erase selected') + if result == 1: + buttonEventHandler(BUTTON_DELETE) + + elif event == Draw.SPACEKEY and value == 1: + + if (G.curempty is not None) and not (G.coordmap.has_key(G.curempty.getName())): + buttonEventHandler(BUTTON_ADD) + + elif event == Draw.ZKEY and value == 1: + + x,y,w,h = getWinRect() + setZoom((w/2, h/2), 1.0) + + elif event == Draw.RKEY and value == 1: + + Draw.Redraw(1) + + +def buttonEventHandler(button): + + """ + Event handler for button presses. + + @param button: Button ID. + """ + + global G + G.havebupclik = False + + if button == BUTTON_QUIT: + removeCustomScriptLink() + writeDataToRegistry() + Draw.Exit() + Window.RedrawAll() + + elif button == BUTTON_LOAD: + G.havebupclik = True + Window.ImageSelector(loadImage) + + elif button == BUTTON_ZOOM: + x,y,w,h = getWinRect() + setZoom((w/2, h/2), G.buttons.zoom.val) + + elif button == BUTTON_FULLOPT: + G.fullopt = G.buttons.fullopt.val + + elif button == BUTTON_COPLANAR: + G.coplanar = G.buttons.coplanar.val + + elif button == BUTTON_OFSX: + G.ofsx = G.buttons.ofsx.val + + elif button == BUTTON_OFSY: + G.ofsy = G.buttons.ofsy.val + + elif button == BUTTON_OFSZ: + G.ofsz = G.buttons.ofsz.val + + elif button == BUTTON_ADD: + G.mode = MODE_ADD + Draw.Redraw(1) + + elif button == BUTTON_DELETE: + def delmap(x): + del G.coordmap[x.getName()] + x.select(False) + map(delmap, G.selection) + Window.RedrawAll() + + elif button == BUTTON_CALIBRATE: + Window.WaitCursor(True) + G.last_camera = G.selection[0] + calibrate() + + +############### +# ENTRY POINT # +############### + +# remove any spurious custom script link that may be +# hanging around from a previous invokation +removeCustomScriptLink() + +# add a new script link +addCustomScriptLink() + +# read any previous coordinate data that has been +# stored +readDataFromRegistry() + +# register event handlers +Draw.Register(renderGUI, eventHandler, buttonEventHandler) \ No newline at end of file diff --git a/others/pyTsai/CHANGELOG b/others/pyTsai/CHANGELOG new file mode 100644 index 0000000..73e9098 --- /dev/null +++ b/others/pyTsai/CHANGELOG @@ -0,0 +1,6 @@ +2012.12.12. Gabor Cseh + +0.1c -> 0.1d: +The python source and the pytsai.c was modified to satisfy the python 3.3 needs. +With the source a new ../dist/PyTsai-1.0.win-amd64.zip file is provided. This is for the python 3.3 version! The same zip file can be found in the pytsai root directory named PyTsai-1.0.win-amd64-python3.3.zip. To install it into python 3.3, you only need to change directory to the directory containts the folder python33 and extract it. After that you can import the Tsai calibration method from the pytyhon command line or from another module using the "import Tsai" (with capital T) command. To install it for Blender 2.65 (for this version it worked, maybe working for other versions too), just copy the content of the folder (just the files, not the whole directory tree) +The binaries for installing it manually for python 3.3 can be found in the ../build directory. You don't need a compiler (in principle), just type "python setup.py install" from the pytsai root folder. \ No newline at end of file diff --git a/others/pyTsai/COPYING b/others/pyTsai/COPYING new file mode 100644 index 0000000..c4792dd --- /dev/null +++ b/others/pyTsai/COPYING @@ -0,0 +1,515 @@ + + GNU LESSER GENERAL PUBLIC LICENSE + Version 2.1, February 1999 + + Copyright (C) 1991, 1999 Free Software Foundation, Inc. + 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + +[This is the first released version of the Lesser GPL. It also counts + as the successor of the GNU Library Public License, version 2, hence + the version number 2.1.] + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +Licenses are intended to guarantee your freedom to share and change +free software--to make sure the software is free for all its users. + + This license, the Lesser General Public License, applies to some +specially designated software packages--typically libraries--of the +Free Software Foundation and other authors who decide to use it. You +can use it too, but we suggest you first think carefully about whether +this license or the ordinary General Public License is the better +strategy to use in any particular case, based on the explanations +below. + + When we speak of free software, we are referring to freedom of use, +not price. Our General Public Licenses are designed to make sure that +you have the freedom to distribute copies of free software (and charge +for this service if you wish); that you receive source code or can get +it if you want it; that you can change the software and use pieces of +it in new free programs; and that you are informed that you can do +these things. + + To protect your rights, we need to make restrictions that forbid +distributors to deny you these rights or to ask you to surrender these +rights. These restrictions translate to certain responsibilities for +you if you distribute copies of the library or if you modify it. + + For example, if you distribute copies of the library, whether gratis +or for a fee, you must give the recipients all the rights that we gave +you. You must make sure that they, too, receive or can get the source +code. If you link other code with the library, you must provide +complete object files to the recipients, so that they can relink them +with the library after making changes to the library and recompiling +it. And you must show them these terms so they know their rights. + + We protect your rights with a two-step method: (1) we copyright the +library, and (2) we offer you this license, which gives you legal +permission to copy, distribute and/or modify the library. + + To protect each distributor, we want to make it very clear that +there is no warranty for the free library. Also, if the library is +modified by someone else and passed on, the recipients should know +that what they have is not the original version, so that the original +author's reputation will not be affected by problems that might be +introduced by others. +^L + Finally, software patents pose a constant threat to the existence of +any free program. We wish to make sure that a company cannot +effectively restrict the users of a free program by obtaining a +restrictive license from a patent holder. Therefore, we insist that +any patent license obtained for a version of the library must be +consistent with the full freedom of use specified in this license. + + Most GNU software, including some libraries, is covered by the +ordinary GNU General Public License. This license, the GNU Lesser +General Public License, applies to certain designated libraries, and +is quite different from the ordinary General Public License. We use +this license for certain libraries in order to permit linking those +libraries into non-free programs. + + When a program is linked with a library, whether statically or using +a shared library, the combination of the two is legally speaking a +combined work, a derivative of the original library. The ordinary +General Public License therefore permits such linking only if the +entire combination fits its criteria of freedom. The Lesser General +Public License permits more lax criteria for linking other code with +the library. + + We call this license the "Lesser" General Public License because it +does Less to protect the user's freedom than the ordinary General +Public License. It also provides other free software developers Less +of an advantage over competing non-free programs. These disadvantages +are the reason we use the ordinary General Public License for many +libraries. However, the Lesser license provides advantages in certain +special circumstances. + + For example, on rare occasions, there may be a special need to +encourage the widest possible use of a certain library, so that it +becomes +a de-facto standard. To achieve this, non-free programs must be +allowed to use the library. A more frequent case is that a free +library does the same job as widely used non-free libraries. In this +case, there is little to gain by limiting the free library to free +software only, so we use the Lesser General Public License. + + In other cases, permission to use a particular library in non-free +programs enables a greater number of people to use a large body of +free software. For example, permission to use the GNU C Library in +non-free programs enables many more people to use the whole GNU +operating system, as well as its variant, the GNU/Linux operating +system. + + Although the Lesser General Public License is Less protective of the +users' freedom, it does ensure that the user of a program that is +linked with the Library has the freedom and the wherewithal to run +that program using a modified version of the Library. + + The precise terms and conditions for copying, distribution and +modification follow. Pay close attention to the difference between a +"work based on the library" and a "work that uses the library". The +former contains code derived from the library, whereas the latter must +be combined with the library in order to run. +^L + GNU LESSER GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License Agreement applies to any software library or other +program which contains a notice placed by the copyright holder or +other authorized party saying it may be distributed under the terms of +this Lesser General Public License (also called "this License"). +Each licensee is addressed as "you". + + A "library" means a collection of software functions and/or data +prepared so as to be conveniently linked with application programs +(which use some of those functions and data) to form executables. + + The "Library", below, refers to any such software library or work +which has been distributed under these terms. A "work based on the +Library" means either the Library or any derivative work under +copyright law: that is to say, a work containing the Library or a +portion of it, either verbatim or with modifications and/or translated +straightforwardly into another language. (Hereinafter, translation is +included without limitation in the term "modification".) + + "Source code" for a work means the preferred form of the work for +making modifications to it. For a library, complete source code means +all the source code for all modules it contains, plus any associated +interface definition files, plus the scripts used to control +compilation +and installation of the library. + + Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running a program using the Library is not restricted, and output from +such a program is covered only if its contents constitute a work based +on the Library (independent of the use of the Library in a tool for +writing it). Whether that is true depends on what the Library does +and what the program that uses the Library does. + + 1. You may copy and distribute verbatim copies of the Library's +complete source code as you receive it, in any medium, provided that +you conspicuously and appropriately publish on each copy an +appropriate copyright notice and disclaimer of warranty; keep intact +all the notices that refer to this License and to the absence of any +warranty; and distribute a copy of this License along with the +Library. + + You may charge a fee for the physical act of transferring a copy, +and you may at your option offer warranty protection in exchange for a +fee. + + 2. You may modify your copy or copies of the Library or any portion +of it, thus forming a work based on the Library, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) The modified work must itself be a software library. + + b) You must cause the files modified to carry prominent notices + stating that you changed the files and the date of any change. + + c) You must cause the whole of the work to be licensed at no + charge to all third parties under the terms of this License. + + d) If a facility in the modified Library refers to a function or a + table of data to be supplied by an application program that uses + the facility, other than as an argument passed when the facility + is invoked, then you must make a good faith effort to ensure that, + in the event an application does not supply such function or + table, the facility still operates, and performs whatever part of + its purpose remains meaningful. + + (For example, a function in a library to compute square roots has + a purpose that is entirely well-defined independent of the + application. Therefore, Subsection 2d requires that any + application-supplied function or table used by this function must + be optional: if the application does not supply it, the square + root function must still compute square roots.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Library, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Library, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote +it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Library. + +In addition, mere aggregation of another work not based on the Library +with the Library (or with a work based on the Library) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may opt to apply the terms of the ordinary GNU General Public +License instead of this License to a given copy of the Library. To do +this, you must alter all the notices that refer to this License, so +that they refer to the ordinary GNU General Public License, version 2, +instead of to this License. (If a newer version than version 2 of the +ordinary GNU General Public License has appeared, then you can specify +that version instead if you wish.) Do not make any other change in +these notices. +^L + Once this change is made in a given copy, it is irreversible for +that copy, so the ordinary GNU General Public License applies to all +subsequent copies and derivative works made from that copy. + + This option is useful when you wish to copy part of the code of +the Library into a program that is not a library. + + 4. You may copy and distribute the Library (or a portion or +derivative of it, under Section 2) in object code or executable form +under the terms of Sections 1 and 2 above provided that you accompany +it with the complete corresponding machine-readable source code, which +must be distributed under the terms of Sections 1 and 2 above on a +medium customarily used for software interchange. + + If distribution of object code is made by offering access to copy +from a designated place, then offering equivalent access to copy the +source code from the same place satisfies the requirement to +distribute the source code, even though third parties are not +compelled to copy the source along with the object code. + + 5. A program that contains no derivative of any portion of the +Library, but is designed to work with the Library by being compiled or +linked with it, is called a "work that uses the Library". Such a +work, in isolation, is not a derivative work of the Library, and +therefore falls outside the scope of this License. + + However, linking a "work that uses the Library" with the Library +creates an executable that is a derivative of the Library (because it +contains portions of the Library), rather than a "work that uses the +library". The executable is therefore covered by this License. +Section 6 states terms for distribution of such executables. + + When a "work that uses the Library" uses material from a header file +that is part of the Library, the object code for the work may be a +derivative work of the Library even though the source code is not. +Whether this is true is especially significant if the work can be +linked without the Library, or if the work is itself a library. The +threshold for this to be true is not precisely defined by law. + + If such an object file uses only numerical parameters, data +structure layouts and accessors, and small macros and small inline +functions (ten lines or less in length), then the use of the object +file is unrestricted, regardless of whether it is legally a derivative +work. (Executables containing this object code plus portions of the +Library will still fall under Section 6.) + + Otherwise, if the work is a derivative of the Library, you may +distribute the object code for the work under the terms of Section 6. +Any executables containing that work also fall under Section 6, +whether or not they are linked directly with the Library itself. +^L + 6. As an exception to the Sections above, you may also combine or +link a "work that uses the Library" with the Library to produce a +work containing portions of the Library, and distribute that work +under terms of your choice, provided that the terms permit +modification of the work for the customer's own use and reverse +engineering for debugging such modifications. + + You must give prominent notice with each copy of the work that the +Library is used in it and that the Library and its use are covered by +this License. You must supply a copy of this License. If the work +during execution displays copyright notices, you must include the +copyright notice for the Library among them, as well as a reference +directing the user to the copy of this License. Also, you must do one +of these things: + + a) Accompany the work with the complete corresponding + machine-readable source code for the Library including whatever + changes were used in the work (which must be distributed under + Sections 1 and 2 above); and, if the work is an executable linked + with the Library, with the complete machine-readable "work that + uses the Library", as object code and/or source code, so that the + user can modify the Library and then relink to produce a modified + executable containing the modified Library. (It is understood + that the user who changes the contents of definitions files in the + Library will not necessarily be able to recompile the application + to use the modified definitions.) + + b) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (1) uses at run time a + copy of the library already present on the user's computer system, + rather than copying library functions into the executable, and (2) + will operate properly with a modified version of the library, if + the user installs one, as long as the modified version is + interface-compatible with the version that the work was made with. + + c) Accompany the work with a written offer, valid for at + least three years, to give the same user the materials + specified in Subsection 6a, above, for a charge no more + than the cost of performing this distribution. + + d) If distribution of the work is made by offering access to copy + from a designated place, offer equivalent access to copy the above + specified materials from the same place. + + e) Verify that the user has already received a copy of these + materials or that you have already sent this user a copy. + + For an executable, the required form of the "work that uses the +Library" must include any data and utility programs needed for +reproducing the executable from it. However, as a special exception, +the materials to be distributed need not include anything that is +normally distributed (in either source or binary form) with the major +components (compiler, kernel, and so on) of the operating system on +which the executable runs, unless that component itself accompanies +the executable. + + It may happen that this requirement contradicts the license +restrictions of other proprietary libraries that do not normally +accompany the operating system. Such a contradiction means you cannot +use both them and the Library together in an executable that you +distribute. +^L + 7. You may place library facilities that are a work based on the +Library side-by-side in a single library together with other library +facilities not covered by this License, and distribute such a combined +library, provided that the separate distribution of the work based on +the Library and of the other library facilities is otherwise +permitted, and provided that you do these two things: + + a) Accompany the combined library with a copy of the same work + based on the Library, uncombined with any other library + facilities. This must be distributed under the terms of the + Sections above. + + b) Give prominent notice with the combined library of the fact + that part of it is a work based on the Library, and explaining + where to find the accompanying uncombined form of the same work. + + 8. You may not copy, modify, sublicense, link with, or distribute +the Library except as expressly provided under this License. Any +attempt otherwise to copy, modify, sublicense, link with, or +distribute the Library is void, and will automatically terminate your +rights under this License. However, parties who have received copies, +or rights, from you under this License will not have their licenses +terminated so long as such parties remain in full compliance. + + 9. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Library or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Library (or any work based on the +Library), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Library or works based on it. + + 10. Each time you redistribute the Library (or any work based on the +Library), the recipient automatically receives a license from the +original licensor to copy, distribute, link with or modify the Library +subject to these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties with +this License. +^L + 11. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Library at all. For example, if a patent +license would not permit royalty-free redistribution of the Library by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Library. + +If any portion of this section is held invalid or unenforceable under +any particular circumstance, the balance of the section is intended to +apply, and the section as a whole is intended to apply in other +circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 12. If the distribution and/or use of the Library is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Library under this License +may add an explicit geographical distribution limitation excluding those +countries, so that distribution is permitted only in or among +countries not thus excluded. In such case, this License incorporates +the limitation as if written in the body of this License. + + 13. The Free Software Foundation may publish revised and/or new +versions of the Lesser General Public License from time to time. +Such new versions will be similar in spirit to the present version, +but may differ in detail to address new problems or concerns. + +Each version is given a distinguishing version number. If the Library +specifies a version number of this License which applies to it and +"any later version", you have the option of following the terms and +conditions either of that version or of any later version published by +the Free Software Foundation. If the Library does not specify a +license version number, you may choose any version ever published by +the Free Software Foundation. +^L + 14. If you wish to incorporate parts of the Library into other free +programs whose distribution conditions are incompatible with these, +write to the author to ask for permission. For software which is +copyrighted by the Free Software Foundation, write to the Free +Software Foundation; we sometimes make exceptions for this. Our +decision will be guided by the two goals of preserving the free status +of all derivatives of our free software and of promoting the sharing +and reuse of software generally. + + NO WARRANTY + + 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO +WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. +EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR +OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY +KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE +LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME +THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN +WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY +AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU +FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR +CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE +LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING +RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A +FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF +SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH +DAMAGES. + + END OF TERMS AND CONDITIONS +^L + How to Apply These Terms to Your New Libraries + + If you develop a new library, and you want it to be of the greatest +possible use to the public, we recommend making it free software that +everyone can redistribute and change. You can do so by permitting +redistribution under these terms (or, alternatively, under the terms +of the ordinary General Public License). + + To apply these terms, attach the following notices to the library. +It is safest to attach them to the start of each source file to most +effectively convey the exclusion of warranty; and each file should +have at least the "copyright" line and a pointer to where the full +notice is found. + + + + Copyright (C) + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Also add information on how to contact you by electronic and paper +mail. + +You should also get your employer (if you work as a programmer) or +your +school, if any, to sign a "copyright disclaimer" for the library, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the + library `Frob' (a library for tweaking knobs) written by James +Random Hacker. + + , 1 April 1990 + Ty Coon, President of Vice + +That's all there is to it! + + diff --git a/others/pyTsai/README.txt b/others/pyTsai/README.txt new file mode 100644 index 0000000..6d2efa1 --- /dev/null +++ b/others/pyTsai/README.txt @@ -0,0 +1,9 @@ +For the old (and somewhat outdated) documentation, please see the HTML file: doc/index.html. For the new, work-in-progress, but +up-to-date documentation, see http://pytsai.readthedocs.org/en/latest/. + +This is an actualized version of pyTsai (for python 3.3 and Blender 2.65) I found the original (for python 2.4, 2.5 and 2.6 +and Blender 2.4) at: http://farmerjoe.info/?p=7 + +To install the Tsai extension for Blender (2.65) (at leat under Windows, which has a separate Python environment), just copy +the contents of the PyTsai-1.0.win-amd64-python3.3.zip into the [...]\Blender Foundation\Blender\2.65\scripts\modules (NOT the +whole folder tree, only the files and the __pycache__ folder). diff --git a/others/pyTsai/Tsai.py b/others/pyTsai/Tsai.py new file mode 100644 index 0000000..19fdbb2 --- /dev/null +++ b/others/pyTsai/Tsai.py @@ -0,0 +1,450 @@ +""" +Routines for calibrating a camera using the method of Tsai. +""" + +import math, string + +import pytsai + + +class CalibrationError(Exception): + """ + Exception used to indicate that an error has occurred during + calibration. + """ + def __init__(self, value): + self.value = value + def __str__(self): + return str(self.value) + + +class CameraParameters: + + """ + Utility class for camera parameters. + + This class can be used as an input and output to the L{calibrate} + method. It functions as a mapping type between camera parameters + (stored as strings), and their values (numbers). + """ + + def __init__(self, existing=None, **keywords): + + """ + @param existing: A mapping type to copy values from. + @keyword model: Specifies an existing (known) camera model. + This can be any of: + - 'photometrics-star-I' + - 'general-imaging-mos5400-matrox' + - 'panasonic-GP-MF702-matrox' + - 'sony-xc75-matrox' + - 'sony-xc77-matrox' + - 'sony-xc57-androx' + - 'xapshot-matrox' + @keyword image_dim: Specifies dimensions of an image to derive + a camera model from. The image_dim should be + a tuple containing M{(width, height)} of the + image. The derived camera model presumes: + - M{Ncx = Nfx = width} + - M{dx = dy = dpx = dpy = 1.0} + - M{Cx = width / 2} + - M{Cy = height / 2} + - M{sx = 1.0} + """ + + # initially create all parameters, setting them to 0.0 + parms = ['Ncx', 'Nfx', 'dx', 'dy', 'dpx', 'dpy', 'Cx', 'Cy', + 'sx', 'f', 'kappa1', 'p1', 'p2', 'Tx', 'Ty', 'Tz', + 'Rx', 'Ry', 'Rz', 'r1', 'r2', 'r3', 'r4', 'r5', + 'r6', 'r7', 'r8', 'r9'] + def createParam(x): self[x]=0.0 + list(map(createParam, parms)) #map() function returns iterator in python 3. This is a workaround. + + # copy an existing map if one was provided + if existing is not None: + for (key,item) in existing.items(): + self[key] = item + + # set the camera model if specified + if 'model' in keywords.keys(): + model = keywords['model'] + if model == 'photometrics-star-I': + self.Ncx = 576 + self.Nfx = 576 + self.dx = 0.023 + self.dy = 0.023 + self.dpx = self.dx * self.Ncx / self.Nfx + self.dpy = self.dy + self.Cx = 258.0 + self.Cy = 204.0 + self.sx = 1.0 + elif model == 'general-imaging-mos5400-matrox': + self.Ncx = 649 + self.Nfx = 512 + self.dx = 0.015 + self.dy = 0.015 + self.dpx = self.dx * self.Ncx / self.Nfx + self.dpy = self.dy + self.Cx = 512/2 + self.Cy = 480/2 + self.sx = 1.0 + elif model == 'panasonic-GP-MF702-matrox': + self.Ncx = 649 + self.Nfx = 512 + self.dx = 0.015 + self.dy = 0.015 + self.dpx = self.dx * self.Ncx / self.Nfx + self.dpy = self.dy + self.Cx = 268 + self.Cy = 248 + self.sx = 1.078647 + elif model == 'sony-xc75-matrox': + self.Ncx = 768 + self.Nfx = 512 + self.dx = 0.0084 + self.dy = 0.0098 + self.dpx = self.dx * self.Ncx / self.Nfx + self.dpy = self.dy + self.Cx = 512 / 2 + self.Cy = 480 / 2 + self.sx = 1.0 + elif model == 'sony-xc77-matrox': + self.Ncx = 768 + self.Nfx = 512 + self.dx = 0.011 + self.dy = 0.013 + self.dpx = self.dx * self.Ncx / self.Nfx + self.dpy = self.dy + self.Cx = 512 / 2 + self.Cy = 480 / 2 + self.sx = 1.0 + elif model == 'sony-xc57-androx': + self.Ncx = 510 + self.Nfx = 512 + self.dx = 0.017 + self.dy = 0.013 + self.dpx = self.dx * self.Ncx / self.Nfx + self.dpy = self.dy + self.Cx = 512 / 2 + self.Cy = 480 / 2 + self.sx = 1.107914 + elif model == 'xapshot-matrox': + self.Ncx = 739 + self.Nfx = 512 + self.dx = 6.4 / 782.0 + self.dy = 4.8 / 250.0 + self.dpx = self.dx * self.Ncx / self.Nfx + self.dpy = self.dy + self.Cx = 512 / 2 + self.Cy = 240 / 2 + self.sx = 1.027753 + + # construct an artificial camera if image_dim has been + # provided + elif 'image_dim' in keywords.keys(): + image_dim = keywords['image_dim'] + self.Ncx = image_dim[0] + self.Nfx = self.Ncx + self.dx = 1.0 + self.dy = self.dx + self.dpx = self.dx + self.dpy = self.dx + self.Cx = image_dim[0] / 2.0 + self.Cy = image_dim[1] / 2.0 + self.sx = 1.0 + + def getPosition(self): + """ + Returns the camera's position as a 3-tuple: (Tx, Ty, Tz). + """ + return (self.Tx, self.Ty, self.Tz) + + def getEulerRotation(self): + """ + Returns the camera's Euler rotation as a 3-tuple: + (Rx, Ry, Rz). + """ + return (self.Rx, self.Ry, self.Rz) + + def getRotationMatrix(self): + """ + Returns the camera's rotation matrix:: + [ + [ r1, r2, r3 ], + [ r4, r5, r6 ], + [ r7, r8, r9 ] + ] + """ + return [ [ self.r1, self.r2, self.r3 ], + [ self.r4, self.r5, self.r6 ], + [ self.r7, self.r8, self.r9 ] ] + + def getFOVx(self): + """ + Returns the camera's x field-of-view angle (in radians). + + This angle is defined as: M{fovx = 2 * atan2(Ncx * dx, 2*f)}. + """ + return 2.0 * math.atan2(self.Ncx * self.dx, 2.0 * self.f); + + def world2image(self, coord): + """ + Converts from world coordinates to image coordinates. The + conversion is based upon the camera's current parameters. + + @param coord: A 3-member sequence containing the world + coordinates M{(xw, yw, zw)}. + + @return: A 2-member sequence containing the image coordinates + M{(xi, yi)}. + """ + return pytsai._pytsai_wc2ic(coord, self) + + def image2world(self, coord): + """ + Converts from image coordinates to world coordinates. The + conversion is based upon the camera's current parameters. + + @param coord: A 3-member sequence containing the image + coordinates and the depth coordinate in the world + M{(xi, yi, zw)}. + + @return: A 3-member sequence containing the world coordinates + M{(xw, yw, zw)}. + """ + return pytsai._pytsai_ic2wc(coord, self) + + def world2camera(self, coord): + """ + Converts from world coordinates to camera coordinates. The + conversion is based upon the camera's current parameters. + + @param coord: A 3-member sequence containing the world + coordinates M{(xw, yw, zw)}. + + @return: A 3-member sequence containing the camera coordinates + M{(xc, yc, zc)}. + """ + xw, yw, zw = coord + xc = self.r1 * xw + self.r2 * yw + self.r3 * zw + self.Tx + yc = self.r4 * xw + self.r5 * yw + self.r6 * zw + self.Ty + zc = self.r7 * xw + self.r8 * yw + self.r9 * zw + self.Tz + return (xc, yc, zc) + + def camera2world(self, coord): + """ + Converts from camera coordinates to world coordinates. The + conversion is based upon the camera's current parameters. + + @param coord: A 3-member sequence containing the camera + coordinates M{(xc, yc, zc)}. + + @return: A 3-member sequence containing the world + coordinates M{(xw, yw, zw)}. + """ + return pytsai._pytsai_cc2wc(coord, self) + + def removeRadialDistortion(self, coord, type='sensor'): + """ + Removes distortion from either image or sensor coordinates. + The conversion is based upon the camera's current + parameters. + + @param coord: A 2-member sequence containing the distorted + image or sensor coordinates. + @param type: The type of the coordinates ('sensor' or + 'image'). + @return: Un-distorted sensor or image coordinates (x,y). + """ + if type == 'sensor': + Xd, Yd = coord + dfac = 1.0 + self.kappa1 * (Xd*Xd + Yd*Yd) + return (Xd*dfac, Yd*dfac) + elif type == 'image': + Xfd,Yfd = coord + Xd = self.dpx * (Xfd - self.Cx) / self.sx + Yd = self.dpy * (Yfd - self.Cy) + Xu,Yu = removeDistortion((Xd,Yd), 'sensor') + Xfu = Xu*self.sx/self.dpx + self.Cx + Yfu = Yu/self.dpy + self.Cy + return (Xfu, Yfu) + else: + raise TypeError('Unknown coordinate type: %s' % \ + type) + + def addRadialDistortion(self, coord, type='sensor'): + """ + Adds distortion to either image or sensor coordinates. + The conversion is based upon the camera's current + parameters. + + @param coord: A 2-member sequence containing the un-distorted + image or sensor coordinates. + @param type: The type of the coordinates ('sensor' or + 'image'). + @return: Distorted sensor or image coordinates (x,y). + """ + if type == 'sensor': + Xu,Yu = coord + return pytsai._pytsai_add_sensor_coord_distortion( + Xu, Yu, self) + elif type == 'image': + Xfu,Yfu = coord + Xu = self.dpx * (Xfu - self.Cx) / self.sx + Yu = self.dpy * (Yfu - self.Cy) + Xd,Yd = addDistortion((Xu, Yu), 'sensor') + Xfd = Xd*self.sx/self.dpx + self.Cx + Yfd = Yd/self.dpy + self.Cy + return (Xfd,Yfd) + else: + raise TypeError('Unknown coordinate type: %s' % \ + type) + + def setBlenderCamera(self, camobj, xres, yres): + """ + This function is now fully compatible with Blender 2.65. + (Haven't tested yet.) + """ + import mathutils + w2c = mathutils.Matrix(( + (self.r1, self.r4, self.r7, 0.0), + (self.r2, self.r5, self.r8, 0.0), + (self.r3, self.r6, self.r9, 0.0), + (self.Tx, self.Ty, self.Tz, 1.0) + )) + rot180x = mathutils.Matrix.Rotation(180, 4, 'X') + c2w = mathutils.Matrix.copy(w2c * rot180x) + c2w.invert() + camobj.matrix_world = c2w + cam = camobj.data + #asp = float(yres) / float(yres) #isn't it a semantic error??? + asp = float(xres) / float(yres) #test with this - maybe its yres/xres + cam.lens = 16.0 / (asp * math.tan(self.getFOVx() / 2.0)) + + def iterkeys(self): + """ + Iterates over the keys of the mapping. + """ + return self.__dict__.iterkeys() + def __getitem__(self, key): + return self.__dict__[key] + def __setitem__(self, key, value): + self.__dict__[key] = value + def __delitem__(self, key): + del self.__dict__[key] + def __iter__(self): + return self.iterkeys() + def __contains__(self, item): + return (item in self.__dict__) + def __str__(self): + str = 'CameraParameters:\n' + parms = ['Ncx', 'Nfx', 'dx', 'dy', 'dpx', 'dpy', 'Cx', 'Cy', + 'sx', 'f', 'kappa1', 'p1', 'p2', 'Tx', 'Ty', 'Tz', + 'Rx', 'Ry', 'Rz', 'r1', 'r2', 'r3', 'r4', 'r5', + 'r6', 'r7', 'r8', 'r9'] + for p in parms: + #s = string.ljust(p, 7) + ("= %f\n" % self[p]) #old, python 2 syntax + s = p.ljust(7) + ("= %f\n" % self[p]) + str += s + return str + def __repr__(self): + return str(self) + + +def calibrate(target_type, optimization_type, calibration_data, camera_params, + origin_offset=(0.0,0.0,0.0)): + """ + Calibrates a camera. + + @param target_type: The type of the target used for calibration. This + can be either: + - 'coplanar', in which all z-values for the calibration + points are zero. + - 'noncoplanar', in which some z-values for the + calibration points must be non-zero. + + @param optimization_type: The type of optimization to perform. This + can be either: + - 'three-param', for optimization of only M{f}, + M{Tz} and M{kappa1}. + - 'full', for full optimization. + + @param calibration_data: A sequence of sequences containing + calibration points. The sequence should consist of:: + [ + [ xs1, ys1, zs1, xi1, yi1 ], + [ xs2, ys2, zs2, xi2, yi2 ], + ... ... ... ... ... + [ xsN, ysN, zsN, xiN, yiN ] + ] + where: + - M{(xs, ys, zs)} are 3D space coordinates of the + calibration points. + - M{(xi, yi)} are corresponding 2D image space + coordinates of the calibration points. + @param camera_params: A dictionary mapping camera parameter names + (stored as strings) to their values (which should be + numbers). The class L{CameraParameters} is a utility class + designed to be used in this position. + + @param origin_offset: An artificial offset that is added to the origin + of the calibration data coordinates. This offset is later + removed from the camera position as determined by the + calibration. Shifting the origin may be useful since the + Tsai method fails if the world space origin is near to the + camera space origin or the camera space y axis. + """ + + # add an origin offset to the camera position + #xo,yo,zo = origin_offset + xo,yo,zo = 0.0,0.0,0.0 + def addOfs(c): + return (c[0]+xo, c[1]+yo, c[2]+zo, c[3], c[4]) + ofsCalData = list(map(addOfs, calibration_data)) + + # perform camera calibration + if target_type == 'coplanar' and optimization_type == 'three-param': + try: + cp = pytsai._pytsai_coplanar_calibration( + ofsCalData, camera_params) + except RuntimeError as runtimeError: + raise CalibrationError(str(runtimeError)) + + elif target_type == 'noncoplanar' and \ + optimization_type == 'three-param': + try: + cp = pytsai._pytsai_noncoplanar_calibration( + ofsCalData, camera_params) + except RuntimeError as runtimeError: + raise CalibrationError(str(runtimeError)) + + elif target_type == 'coplanar' and optimization_type == 'full': + try: + cp = pytsai._pytsai_coplanar_calibration_fo( + ofsCalData, camera_params) + except RuntimeError as runtimeError: + raise CalibrationError(str(runtimeError)) + + elif target_type == 'noncoplanar' and optimization_type == 'full': + try: + cp = pytsai._pytsai_noncoplanar_calibration_fo( + ofsCalData, camera_params) + except RuntimeError as runtimeError: + raise CalibrationError(str(runtimeError)) + + else: + errstr = 'Unknown combination of target_type=\'%s\' and ' \ + 'optimization_type=\'%s\'' % \ + (target_type, optimization_type) + raise CalibrationError(errstr) + + # remove the origin offset from the camera position + ccp = CameraParameters(cp) + #camorigin = ccp.world2camera((xo,yo,zo)) + #ccp.Tx -= camorigin[0] + #ccp.Ty -= camorigin[1] + #ccp.Tz -= camorigin[2] + + # return the calculated camera parameters + return ccp + diff --git a/others/pyTsai/doc/calibrate-camera.jpg b/others/pyTsai/doc/calibrate-camera.jpg new file mode 100644 index 0000000..c1bd575 Binary files /dev/null and b/others/pyTsai/doc/calibrate-camera.jpg differ diff --git a/others/pyTsai/doc/calimg.tga b/others/pyTsai/doc/calimg.tga new file mode 100644 index 0000000..df541d6 Binary files /dev/null and b/others/pyTsai/doc/calimg.tga differ diff --git a/others/pyTsai/doc/error.py b/others/pyTsai/doc/error.py new file mode 100644 index 0000000..f5557db --- /dev/null +++ b/others/pyTsai/doc/error.py @@ -0,0 +1,44 @@ +#---------------------------------------------------------- +# File error.py +# Simple error dialog +#---------------------------------------------------------- + +import bpy +from bpy.props import * + +# +# The error message operator. When invoked, pops up a dialog +# window with the given message. +# +class MessageOperator(bpy.types.Operator): + bl_idname = "error.message" + bl_label = "Message" + message = StringProperty() + + def execute(self, context): + self.report({'INFO'}, self.message) + print(self.message) + return {'FINISHED'} + + def invoke(self, context, event): + wm = context.window_manager + return wm.invoke_popup(self, width=400, height=200) + + def draw(self, context): + self.layout.label("ERROR") + row = self.layout.split(1.0) + row.prop(self, "message") + +# Register classes and start scan automatically +bpy.utils.register_class(MessageOperator) + +def menuError(str): + """ + Displays an error string in a pop-up menu. + """ + bpy.ops.error.message('INVOKE_DEFAULT', + message = str) + return + +if __name__ == "__main__": + menuError('valamihiba') \ No newline at end of file diff --git a/others/pyTsai/doc/image-loaded.jpg b/others/pyTsai/doc/image-loaded.jpg new file mode 100644 index 0000000..af8d891 Binary files /dev/null and b/others/pyTsai/doc/image-loaded.jpg differ diff --git a/others/pyTsai/doc/index.html b/others/pyTsai/doc/index.html new file mode 100644 index 0000000..2f49c61 --- /dev/null +++ b/others/pyTsai/doc/index.html @@ -0,0 +1,286 @@ + + + + + Camera Calibration for Blender + + +

Camera Calibration for Blender

+

Download (Note: Site is no longer live)

+You can get the latest version of the extension module and script +bundle here (includes everything, even this documentation): http://www.warpax.com/pytsai/pytsai-0.1.tar.gz
+
+ +

Download (With Binaries added by lobo_nz)

+You can get version 0.1 of the extension module and script +bundle here (includes everything, even this documentation) +along with compiled binaries for linux and windows for python 2.4, 2.5 and 2.6 (No 2.6 for Linux sorry but you guys should be able to easily compile it). +Also now with support for loading images other than Raw Targa added by pix http://pix.test.at/ to the BlenderTsai.py +The original blender gui script is still included as BlenderTsai_original.py
+This release is labeled 0.1c to distinguish from the original package and the previous packages I released: http://www.farmerjoe.info
+
+ +Once you've read this file, check out the tutorial: tutorial.html
+

Introduction

+Welcome to the wonderful world of camera calibration, direct from the +comfort of your favourite 3D studio program: Blender!
+
+So what is camera calibration? Well, imagine that you have a real +camera in the real world, and you have taken a picture of a real scene. +Now imagine that you have modeled at least some of that real scene in +Blender, using dimensions and angles from the real world. Let's suppose +that you want to put a camera at the same location in the Blender +scene, relative to the modeled objects, as the real camera occupied in +the real scene, relative to the real objects. That's what camera +calibration does for you! You can think of it as setting up a camera in +a virtual scene at the same spot as the camera in a real scene. I'm +sure you can think of many fun uses for that kind of technology, +ne? :-)  In the world of visual effects (I'm speaking from the +experience of watching many DVD extras disks here :-), I've heard this +method referred to by various names including camera matching and camera tracking. The current +technique only works for still images so far, so it doesn't truly do +camera "tracking" just yet (unless you're into manual labour in a big way!).
+
+Calibrating the Camera
+
+In the current method, the parts of the scene that are modeled in +Blender must be Empties. This is because they represent point +locations. In the real world, they can be the corners of shapes, +specially designed markers (eg: ping-pong balls), the corners of a +checkerboard pattern on a plane, etc. These points, whose 3D locations +are known, are called the calibration +geometry or calibration target.  +Not all calibration geometry in the literature are point sets: +sometimes other things such as sets of line segments are used. However, +we are +restricted to points in the current method. The task of matching up the +points in the virtual scene with the points in the image of the real +scene is called the correspondence +problem, and is solved manually here. The user +of the script must select the Blender Empties and assign them a +corresponding point in the image. Naturally, a funky GUI is supplied +for just this purpose.
+
+Once the calibration target has been defined and corresponding image +space points have been assigned, the camera can then be calibrated. The +method determines two main sets of data about the camera. Firstly, it +determines the position and orientation of the camera, which is a 6 +degree-of-freedom (DOF) quantity. The position and orientation are +sometimes referred to as the exterior +orientation or exterior +parameters of the camera. The method also determines the Blender +lens +value of the camera, which is the interior +orientation or interior +parameter.  Finally, the method does a first-order +approximation for radial distortion of the image, supplying a centre of +distortion and a single distortion constant. These last three +parameters (Cx, Cy, kappa1) can be retrieved quite simply if required, +but are currently ignored because Blender has no representation for +them in its own camera model.
+ +

What Makes Camera +Calibration Difficult and How Do We Do It?

+If we take two sets of 3D points, and are asked to find a rigid body +transformation which makes them align, it is a relatively easy task. +There is an exact solutions available for three points, and a number of +strategies available for minimizing the error from more than three +points. A +good, general, least-squares technique is given in this paper:
+
+
Challis, J.H. (1995) A Procedure for +Determining Rigid Body Transformation Parameters, J. Biomech. +28(6):733-737.
+
+However, the problem in camera calibration is quite different. In +camera calibration, we have lost +information about the 3D scene: specifically, how far the points are +away from the camera, or their depth. +We are now no longer trying to find a mapping from 3D to 3D (which as +pointed out above is quite simple), but instead we are trying to find a +mapping from 3D to 2D. This loss of information complicates the problem +tremendously!
+
+The technique used here was described by Roger Tsai:
+
+
Tsai, R.Y. (1986) An Efficient and +Accurate Camera Calibration Technique for 3D Machine Vision. +Proceedings of IEEE Conference on Computer Vision and Pattern +Recognition, Miami Beach, FL, pp. 364-374.
+
+Tsai, R.Y. (1987) A Versatile Camera Calibration Technique for +High-Accuracy 3D Machine Vision Metrology Using Off-the-Shelf TV +Cameras and Lenses. IEEE Journal of Robotics and Automation, Vol. RA-3, +No. 4, pp. 323-344.
+
+Incidentally, if anyone has complete copies of those +papers, can you please +(please, please, please...) send +me copies?!  I have partial, very poorly-photocopied versions. The +code used as the basis for the current implementation was written by +Reg Willson et al., and is available online here:
+
+ +
+I simply wrote a Python wrapper around the C code available on that +website, and modified it to continue (returning an error flag) when an +error occurs, rather than bailing out instantly using the exit() +function.
+

Using the Camera Calibration Script

+The camera calibration script depends upon a Python extension module +which is a wrapper around the code listed above. This wrapper defines a +Python module called Tsai, and a lower-level C module +called pytsai. Before you can use the Blender script, you +must therefore install the extension module. This is done using the +normal distutils. Under Linux (sorry Windoze users, I +must admit ignorance of your requirements):
+
+
$ python setup.py build
+$ su
+# python setup.py install
+
+Then you can fire up Blender:
+
    +
  1. Set up a scene with some Empties placed at the location of +calibration points. The Empties do not need to be specially named or +anything, and can be arbitrarily connected to other geometry in the +scene (parented, etc.). The Empties can optionally be added later while +the script is running, but I tend to set them up in advance and save +the file in case the script segfaults on me or something (grin).
    +
    2. +
  2. Load and run the script (use the menus of the Text window if you +can't remember the shortcuts - I can't).
    3. +
  3. Click on the Load Image +button to load an image to use for the calibration. (IMPORTANT: The +image must be in RAW TGA +format for now.) The image can be moved in the script window by +dragging with the middle mouse button. You can change the zoom using +either the mouse wheel or (perhaps if you don't have a mouse wheel) the + Zoom numeric +button. Pressing ZKEY will reset the zoom to 1.0.
    +
    4. +
  4. Select Empties in the 3D scene (in the 3D view) and add +calibration points to the image. If everything is working correctly, +when you select an Empty in the 3D view, the script window will change +automatically to give you the option of adding a corresponding point to +the image. Points can be added by clicking the Add button or by pressing +the SPACEBAR. This will bring up a pair of red cross-hairs that show +the prospective location of the image point. Note that the offset of +the cross-hairs from the mouse pointer is deliberate; it is designed to +assist in seeing the actual intersection of the two lines (yes, I know +it can be off-putting, but you get used to it). When working with image +points after they have been added, GKEY, AKEY and XKEY work as +expected. The only minor point is that GKEY drag mode cannot be +cancelled by pressing ESCKEY - you must actually move the point(s) +somewhere and finalize the drag once you have started it.  If you +require fine-tuning of position that is cumbersome with the mouse, you +can use the arrow keys to move around one screen pixel at a time. +Points can be selected using the right mouse button, and multiple +selections can be achieved by holding down either SHIFT key. Points can +be deleted using the Delete +button (which does not prompt +for confirmation) or by pressing the XKEY (which does prompt for comfirmation).
    +
    5. +
  5. Once correspondence has been achieved between scene Empties and +the image points, calibrate the camera. When you select or create a new +camera, the script should change to offer calibration options. Set the +options as you require (see the Notes below for some hints) and click +the Calibrate button.
    6. +
+

Notes

+The following are some general notes about using the script. This +documentation is far from complete, but should hopefully give you +enough to be going on with:
+
    +
  • Two types of calibration targets are possible. You must use the +correct calibration script option for each target type, otherwise the +calibration will definitely fail (or be wildly inaccurate):
    +
    • +
      +
    • Coplanar - all calibration points lie in the same plane (they must have z = 0 in their 3D space +coordinates, and must not +approximate a line).
      • +
    • Noncoplanar - calibration points occupy 3D space (they must not approximate a line or +plane).
      • +
    +
  • Two calibration routines exist:
    • +
      +
    • Partial optimization - non-linear optimization is performed +only for the Tz, f, and kappa1 +parameters (which are the z-camera coordinate, focal length and radial +distortion constant respectively).
      • +
    • Full optimization - non-linear optimization is performed for +all parameters. This method currently seems to have some kind of bug, +which I'm looking into (I may have introduced a bug myself during my +wild hacking of Willson's code).
      +
      • +
    +
  • Calibration will fail if the world space origin is near to the +vertical line that passes through the centre of the image. For now, +just avoid this condition (don't have the scene origin anywhere near +the vertical centre of the image).  You can manually offset the +scene if you need to. In the future, an option will be provided to +mathematically shift the origin of the entire scene during calibration +and then move it back again afterwards if necessary.
    • +
  • For coplanar calibration, avoid looking directly along the z-axis +(ie: straight down at the target). If you do so, it is not possible to +distinguish depth effects from focal length effects. Your calibration +may fail or be wildly inaccurate. Willson recommends a minimum angle of +30 degrees from the z-axis, although that is arbitrary.
    • +
  • To increase the accuracy of calibration, for both target types, +consider the following:
    • +
      +
    • "Perspective effects" increase the accuracy. These can be +achieved by having calibration points located over a wide range of +depths in the scene. This will particularly help to pin down the camera +z-coordinate and its focal length.
      • +
    • More points will increase the calibration accuracy. Be creative +about including more points in your calibration set (provided they are +accurate). If, for example, you have a pole with a calibration point at +the top, place a few calibration points down along its length. For +coplanar checkerboard targets, use a denser grid of checkers. If you +are indoors and have a regular grid pattern of ceiling tiles, maybe +include their corners in your set. Always beware, though, that +architecture is sometimes notoriously non-square! If in doubt, measure +things using reliable techniques first.
      • +
    +
  • The Python extension module contains many externally-visible +symbols (many non-static functions, and three main global variables), +instead of just the module init function. The chances of a naming +conflict occurring between these symbols and symbols in either the +Python interpreter or Blender itself seems to be quite remote (I +changed the names of the three global variables to something more +"specific" just in case), but it is worth me noting this problem +anyway. The Python documentation is very strict about this issue: these +symbols should not be visible. +The only problem is that enforcing that condition would require some +major re-working of the library. Maybe there is a shortcut that I've +missed? C++ namespaces maybe? Please let me know if you are aware of an +easy solution.
    • +
+
+Jonathan Merritt (j.merritt@pgrad.unimelb.edu.au), +
+PhD Student (Equine Biomechanics),
+The University of Melbourne +Equine Centre,
+240 Princes Highway,
+Werribee, Vic. 3030.
+
+Last updated: 04-Nov-2004 (20041104).
+ + diff --git a/others/pyTsai/doc/index.html~ b/others/pyTsai/doc/index.html~ new file mode 100644 index 0000000..3f7a929 --- /dev/null +++ b/others/pyTsai/doc/index.html~ @@ -0,0 +1,286 @@ + + + + + Camera Calibration for Blender + + +

Camera Calibration for Blender

+

Download (Note: Site is no longer live)

+You can get the latest version of the extension module and script +bundle here (includes everything, even this documentation): http://www.warpax.com/pytsai/pytsai-0.1.tar.gz
+
+ +

Download (With Binaries added by lobo_nz)

+You can get version 0.1 of the extension module and script +bundle here (includes everything, even this documentation) +along with compiled binaries for linux and windows for python 2.4 and 2.5. +Also now with support for loading images other than Raw Targa added by pix http://pix.test.at/ to the BlenderTsai.py +The original blender gui script is still included as BlenderTsai_original.py
+This release is labeled 0.1b to distinguish from the original package and the previous package I released: http://blender.formworks.co.nz
+
+ +Once you've read this file, check out the tutorial: tutorial.html
+

Introduction

+Welcome to the wonderful world of camera calibration, direct from the +comfort of your favourite 3D studio program: Blender!
+
+So what is camera calibration? Well, imagine that you have a real +camera in the real world, and you have taken a picture of a real scene. +Now imagine that you have modeled at least some of that real scene in +Blender, using dimensions and angles from the real world. Let's suppose +that you want to put a camera at the same location in the Blender +scene, relative to the modeled objects, as the real camera occupied in +the real scene, relative to the real objects. That's what camera +calibration does for you! You can think of it as setting up a camera in +a virtual scene at the same spot as the camera in a real scene. I'm +sure you can think of many fun uses for that kind of technology, +ne? :-)  In the world of visual effects (I'm speaking from the +experience of watching many DVD extras disks here :-), I've heard this +method referred to by various names including camera matching and camera tracking. The current +technique only works for still images so far, so it doesn't truly do +camera "tracking" just yet (unless you're into manual labour in a big way!).
+
+Calibrating the Camera
+
+In the current method, the parts of the scene that are modeled in +Blender must be Empties. This is because they represent point +locations. In the real world, they can be the corners of shapes, +specially designed markers (eg: ping-pong balls), the corners of a +checkerboard pattern on a plane, etc. These points, whose 3D locations +are known, are called the calibration +geometry or calibration target.  +Not all calibration geometry in the literature are point sets: +sometimes other things such as sets of line segments are used. However, +we are +restricted to points in the current method. The task of matching up the +points in the virtual scene with the points in the image of the real +scene is called the correspondence +problem, and is solved manually here. The user +of the script must select the Blender Empties and assign them a +corresponding point in the image. Naturally, a funky GUI is supplied +for just this purpose.
+
+Once the calibration target has been defined and corresponding image +space points have been assigned, the camera can then be calibrated. The +method determines two main sets of data about the camera. Firstly, it +determines the position and orientation of the camera, which is a 6 +degree-of-freedom (DOF) quantity. The position and orientation are +sometimes referred to as the exterior +orientation or exterior +parameters of the camera. The method also determines the Blender +lens +value of the camera, which is the interior +orientation or interior +parameter.  Finally, the method does a first-order +approximation for radial distortion of the image, supplying a centre of +distortion and a single distortion constant. These last three +parameters (Cx, Cy, kappa1) can be retrieved quite simply if required, +but are currently ignored because Blender has no representation for +them in its own camera model.
+ +

What Makes Camera +Calibration Difficult and How Do We Do It?

+If we take two sets of 3D points, and are asked to find a rigid body +transformation which makes them align, it is a relatively easy task. +There is an exact solutions available for three points, and a number of +strategies available for minimizing the error from more than three +points. A +good, general, least-squares technique is given in this paper:
+
+
Challis, J.H. (1995) A Procedure for +Determining Rigid Body Transformation Parameters, J. Biomech. +28(6):733-737.
+
+However, the problem in camera calibration is quite different. In +camera calibration, we have lost +information about the 3D scene: specifically, how far the points are +away from the camera, or their depth. +We are now no longer trying to find a mapping from 3D to 3D (which as +pointed out above is quite simple), but instead we are trying to find a +mapping from 3D to 2D. This loss of information complicates the problem +tremendously!
+
+The technique used here was described by Roger Tsai:
+
+
Tsai, R.Y. (1986) An Efficient and +Accurate Camera Calibration Technique for 3D Machine Vision. +Proceedings of IEEE Conference on Computer Vision and Pattern +Recognition, Miami Beach, FL, pp. 364-374.
+
+Tsai, R.Y. (1987) A Versatile Camera Calibration Technique for +High-Accuracy 3D Machine Vision Metrology Using Off-the-Shelf TV +Cameras and Lenses. IEEE Journal of Robotics and Automation, Vol. RA-3, +No. 4, pp. 323-344.
+
+Incidentally, if anyone has complete copies of those +papers, can you please +(please, please, please...) send +me copies?!  I have partial, very poorly-photocopied versions. The +code used as the basis for the current implementation was written by +Reg Willson et al., and is available online here:
+
+ +
+I simply wrote a Python wrapper around the C code available on that +website, and modified it to continue (returning an error flag) when an +error occurs, rather than bailing out instantly using the exit() +function.
+

Using the Camera Calibration Script

+The camera calibration script depends upon a Python extension module +which is a wrapper around the code listed above. This wrapper defines a +Python module called Tsai, and a lower-level C module +called pytsai. Before you can use the Blender script, you +must therefore install the extension module. This is done using the +normal distutils. Under Linux (sorry Windoze users, I +must admit ignorance of your requirements):
+
+
$ python setup.py build
+$ su
+# python setup.py install
+
+Then you can fire up Blender:
+
    +
  1. Set up a scene with some Empties placed at the location of +calibration points. The Empties do not need to be specially named or +anything, and can be arbitrarily connected to other geometry in the +scene (parented, etc.). The Empties can optionally be added later while +the script is running, but I tend to set them up in advance and save +the file in case the script segfaults on me or something (grin).
    +
    2. +
  2. Load and run the script (use the menus of the Text window if you +can't remember the shortcuts - I can't).
    3. +
  3. Click on the Load Image +button to load an image to use for the calibration. (IMPORTANT: The +image must be in RAW TGA +format for now.) The image can be moved in the script window by +dragging with the middle mouse button. You can change the zoom using +either the mouse wheel or (perhaps if you don't have a mouse wheel) the + Zoom numeric +button. Pressing ZKEY will reset the zoom to 1.0.
    +
    4. +
  4. Select Empties in the 3D scene (in the 3D view) and add +calibration points to the image. If everything is working correctly, +when you select an Empty in the 3D view, the script window will change +automatically to give you the option of adding a corresponding point to +the image. Points can be added by clicking the Add button or by pressing +the SPACEBAR. This will bring up a pair of red cross-hairs that show +the prospective location of the image point. Note that the offset of +the cross-hairs from the mouse pointer is deliberate; it is designed to +assist in seeing the actual intersection of the two lines (yes, I know +it can be off-putting, but you get used to it). When working with image +points after they have been added, GKEY, AKEY and XKEY work as +expected. The only minor point is that GKEY drag mode cannot be +cancelled by pressing ESCKEY - you must actually move the point(s) +somewhere and finalize the drag once you have started it.  If you +require fine-tuning of position that is cumbersome with the mouse, you +can use the arrow keys to move around one screen pixel at a time. +Points can be selected using the right mouse button, and multiple +selections can be achieved by holding down either SHIFT key. Points can +be deleted using the Delete +button (which does not prompt +for confirmation) or by pressing the XKEY (which does prompt for comfirmation).
    +
    5. +
  5. Once correspondence has been achieved between scene Empties and +the image points, calibrate the camera. When you select or create a new +camera, the script should change to offer calibration options. Set the +options as you require (see the Notes below for some hints) and click +the Calibrate button.
    6. +
+

Notes

+The following are some general notes about using the script. This +documentation is far from complete, but should hopefully give you +enough to be going on with:
+
    +
  • Two types of calibration targets are possible. You must use the +correct calibration script option for each target type, otherwise the +calibration will definitely fail (or be wildly inaccurate):
    +
    • +
      +
    • Coplanar - all calibration points lie in the same plane (they must have z = 0 in their 3D space +coordinates, and must not +approximate a line).
      • +
    • Noncoplanar - calibration points occupy 3D space (they must not approximate a line or +plane).
      • +
    +
  • Two calibration routines exist:
    • +
      +
    • Partial optimization - non-linear optimization is performed +only for the Tz, f, and kappa1 +parameters (which are the z-camera coordinate, focal length and radial +distortion constant respectively).
      • +
    • Full optimization - non-linear optimization is performed for +all parameters. This method currently seems to have some kind of bug, +which I'm looking into (I may have introduced a bug myself during my +wild hacking of Willson's code).
      +
      • +
    +
  • Calibration will fail if the world space origin is near to the +vertical line that passes through the centre of the image. For now, +just avoid this condition (don't have the scene origin anywhere near +the vertical centre of the image).  You can manually offset the +scene if you need to. In the future, an option will be provided to +mathematically shift the origin of the entire scene during calibration +and then move it back again afterwards if necessary.
    • +
  • For coplanar calibration, avoid looking directly along the z-axis +(ie: straight down at the target). If you do so, it is not possible to +distinguish depth effects from focal length effects. Your calibration +may fail or be wildly inaccurate. Willson recommends a minimum angle of +30 degrees from the z-axis, although that is arbitrary.
    • +
  • To increase the accuracy of calibration, for both target types, +consider the following:
    • +
      +
    • "Perspective effects" increase the accuracy. These can be +achieved by having calibration points located over a wide range of +depths in the scene. This will particularly help to pin down the camera +z-coordinate and its focal length.
      • +
    • More points will increase the calibration accuracy. Be creative +about including more points in your calibration set (provided they are +accurate). If, for example, you have a pole with a calibration point at +the top, place a few calibration points down along its length. For +coplanar checkerboard targets, use a denser grid of checkers. If you +are indoors and have a regular grid pattern of ceiling tiles, maybe +include their corners in your set. Always beware, though, that +architecture is sometimes notoriously non-square! If in doubt, measure +things using reliable techniques first.
      • +
    +
  • The Python extension module contains many externally-visible +symbols (many non-static functions, and three main global variables), +instead of just the module init function. The chances of a naming +conflict occurring between these symbols and symbols in either the +Python interpreter or Blender itself seems to be quite remote (I +changed the names of the three global variables to something more +"specific" just in case), but it is worth me noting this problem +anyway. The Python documentation is very strict about this issue: these +symbols should not be visible. +The only problem is that enforcing that condition would require some +major re-working of the library. Maybe there is a shortcut that I've +missed? C++ namespaces maybe? Please let me know if you are aware of an +easy solution.
    • +
+
+Jonathan Merritt (j.merritt@pgrad.unimelb.edu.au), +
+PhD Student (Equine Biomechanics),
+The University of Melbourne +Equine Centre,
+240 Princes Highway,
+Werribee, Vic. 3030.
+
+Last updated: 04-Nov-2004 (20041104).
+ + diff --git a/others/pyTsai/doc/index.rst b/others/pyTsai/doc/index.rst new file mode 100644 index 0000000..97b11aa --- /dev/null +++ b/others/pyTsai/doc/index.rst @@ -0,0 +1,9 @@ +====== +pyTsai - automated camera calibration in Python +====== + +This documentation is created to give a detailed insight in the heart of pyTsai, Python bindings for the Tsai calibration method and the Tsai calibration method itself (especially in the C reference implementation, which is used here). + + +How to use the Tsai calibration method in Python. +----- diff --git a/others/pyTsai/doc/openImage.py b/others/pyTsai/doc/openImage.py new file mode 100644 index 0000000..30127f4 --- /dev/null +++ b/others/pyTsai/doc/openImage.py @@ -0,0 +1,29 @@ +import bpy + +class OpenImage(bpy.types.Operator): + bl_idname = "dialog.open_image" # this is important since its how bpy.ops.dialog.open_file is constructed + bl_label = "Open Image" + filepath = bpy.props.StringProperty(subtype="FILE_PATH") + x = bpy.props.IntProperty() + y = bpy.props.IntProperty() + + def invoke(self, context, event): + wm = context.window_manager.fileselect_add(self) + self.x = event.mouse_x + self.y = event.mouse_y + return {'RUNNING_MODAL'} + + def execute(self, context): + # rather then printing, use the report function, + # this way the messag appiers in the header, + #self.report({'INFO'}, "Mouse coords are %d %d" % (self.x, self.y)) + #self.report({'INFO'},"The file path is %s" % (self.filepath)) + bpy.ops.image.open(filepath=self.filepath) + bpy.data.texts.new('image_filepath') + bpy.data.texts['image_filepath'].write(self.filepath) + #self.report({'INFO'},"The file path is %s" % (self.filepath)) + #self.report({'INFO'},"The images are %s" % (bpy.data.images.keys())) + return {'FINISHED'} + +bpy.utils.register_class(OpenImage) +#bpy.ops.dialog.open_image('INVOKE_DEFAULT') diff --git a/others/pyTsai/doc/placing-point.jpg b/others/pyTsai/doc/placing-point.jpg new file mode 100644 index 0000000..c3d5240 Binary files /dev/null and b/others/pyTsai/doc/placing-point.jpg differ diff --git a/others/pyTsai/doc/tutorial.blend b/others/pyTsai/doc/tutorial.blend new file mode 100644 index 0000000..b1cc06b Binary files /dev/null and b/others/pyTsai/doc/tutorial.blend differ diff --git a/others/pyTsai/doc/tutorial.html b/others/pyTsai/doc/tutorial.html new file mode 100644 index 0000000..9a7f04b --- /dev/null +++ b/others/pyTsai/doc/tutorial.html @@ -0,0 +1,99 @@ + + + + + Camera Calibration Tutorial + + +

Camera Calibration Tutorial

+
+This tutorial takes you through the task of calibrating a camera using +a collinear calibration target.  Please read the Main Documentation if you haven't yet done so. +The image that is used for calibration is generated as a Blender +rendering (it is synthetic), and therefore allows for direct comparison +of the results of the calibration.
+

1. Load the Tutorial

+
    +
  1. Start Blender.
    2. +
  2. Load the file tutorial.blend.
    3. +
+

2. Examine the Scene

+
    +
  1. Layer 1 contains the geometry of the rendered scene.
    2. +
  2. Layer 2 contains Empties that comprise the calibration target, +and a camera to calibrate.
    3. +
  3. Layer 3 contains the original camera and light source used to +render the synthetic image.
    4. +
+

3. Use the BlenderTsai.py Script

+
    +
  1. Use Alt-P in the text window to start the script. Note that while +you're using the script, you will need to be able to view the 3D view +in addition to the script window. The 3D window is used to select +empties to use as calibration points.
    +
    2. +
  2. Click the Load Image +button and load the doc/calimg.tga image. This image is +stored as a RAW TGA file because the script can only load RAW TGA files:
    3. +
+
Loaded Image
+
+
    +
  1. Select the Empty with the largest y-coordinate and x=0.  +This empty corresponds to the top left corner of the checkerboard +pattern. Move the mouse over to the script window and press SPACEBAR to +add an image point.  You can zoom using the mouse scroll wheel or +the zoom numeric button, and the image can be moved around by dragging +with the middle mouse button. The cross-hairs show the point's +prospective location. Click with the left mouse button to place the +point:
    +
    2. +
+
Placing a Point
+
+
    +
  1. If you need to move a point that has been placed already, you can +invoke grab mode on it by pressing GKEY. You can select points using +the right mouse button, and multiple selections can be achieved by +holding down either SHIFT key. When working with the image points, +GKEY, AKEY and XKEY work as expected. The only thing to note is that if +you start a drag with GKEY, you must complete it; you cannot cancel by +pressing ESCKEY. Points can additionally be deleted from the image by +pressing the Delete +button. Note that while the XKEY prompts before deleting points, the Delete button does not +prompt.
    2. +
  2. Continue placing points on the image until each calibration Empty +has a corresponding image point.
    3. +
  3. Select the camera to calibrate and click the Calibrate button in the +script window (the default options are what you need - see below):
    4. +
+
Calibrate the Camera
+
+
    +
  1. If everything has gone to planned, then the camera you have just +calibrated will match (closely) the camera originally used to render +the image. Check both the positions, orientations and the lens values +of the two cameras.
    2. +
+
+Jonathan Merritt (j.merritt@pgrad.unimelb.edu.au), +
+PhD Student (Equine Biomechanics),
+The University of Melbourne +Equine Centre,
+240 Princes Highway,
+Werribee, Vic. 3030.
+
+Last updated: 04-Nov-2004 (20041104).
+ + diff --git a/others/pyTsai/doc/tutorial2.65.blend b/others/pyTsai/doc/tutorial2.65.blend new file mode 100644 index 0000000..d1ea8a9 Binary files /dev/null and b/others/pyTsai/doc/tutorial2.65.blend differ diff --git a/others/pyTsai/doc/tutorial2.65.blend1 b/others/pyTsai/doc/tutorial2.65.blend1 new file mode 100644 index 0000000..aecde9d Binary files /dev/null and b/others/pyTsai/doc/tutorial2.65.blend1 differ diff --git a/others/pyTsai/doc/tutorial2.65.blend2 b/others/pyTsai/doc/tutorial2.65.blend2 new file mode 100644 index 0000000..37dacd2 Binary files /dev/null and b/others/pyTsai/doc/tutorial2.65.blend2 differ diff --git a/others/pyTsai/main.cpp b/others/pyTsai/main.cpp new file mode 100644 index 0000000..6002b69 --- /dev/null +++ b/others/pyTsai/main.cpp @@ -0,0 +1,200 @@ +#include "tsai/cal_main.h" +#include + +#include +#include +#include + + +void show_errors(std::string window_name)//, calibration_data tsai_cd, camera_parameters tsai_cp) +{ + double Xf, Yf; + cv::Mat preview = cv::Mat::zeros(tsai_cp.Ncy, tsai_cp.Ncx, CV_8UC3); + for (int i = 0; i < tsai_cd.point_count; i++) + { + /* calculate the ideal location of the image of the data point */ + world_coord_to_image_coord(tsai_cd.xw[i], tsai_cd.yw[i], tsai_cd.zw[i], &Xf, &Yf); + cv::circle(preview, cv::Point(Xf, Yf), 10, cv::Scalar(0, 0, 255), 2); + cv::circle(preview, cv::Point(tsai_cd.Xf[i], tsai_cd.Yf[i]), 10, cv::Scalar(255, 0), 2); + } + cv::imshow(window_name, preview); + cv::waitKey(0); +} + + +void set_data() +{ + //set data + std::vector> data = { +#include "points.data" + }; + tsai_cd.point_count = data.size(); + for (size_t i = 0; i < data.size(); i++) + { + tsai_cd.xw[i] = data[i][0]; + tsai_cd.yw[i] = data[i][1]; + tsai_cd.zw[i] = data[i][2]; + tsai_cd.Xf[i] = data[i][3]; + tsai_cd.Yf[i] = data[i][4]; + } +} + +void set_inner_data() +{ + //set data + std::vector> data = { +#include "points_inner.data" + }; + tsai_cd.point_count = data.size(); + for (size_t i = 0; i < data.size(); i++) + { + tsai_cd.xw[i] = data[i][0]; + tsai_cd.yw[i] = data[i][1]; + tsai_cd.zw[i] = data[i][2]; + tsai_cd.Xf[i] = data[i][3]; + tsai_cd.Yf[i] = data[i][4]; + } +} + + +static cv::Mat angle2mat(const double& Rx, const double& Ry, const double& Rz) +{ + double sa = sin(Rx); + double ca = cos(Rx); + double sb = sin(Ry); + double cb = cos(Ry); + double sg = sin(Rz); + double cg = cos(Rz); + + cv::Mat rm(3, 3, CV_64FC1); + + rm.at(0, 0) = cb * cg; + rm.at(0, 1) = cg * sa * sb - ca * sg; + rm.at(0, 2) = sa * sg + ca * cg * sb; + rm.at(1, 0) = cb * sg; + rm.at(1, 1) = sa * sb * sg + ca * cg; + rm.at(1, 2) = ca * sb * sg - cg * sa; + rm.at(2, 0) = -sb; + rm.at(2, 1) = cb * sa; + rm.at(2, 2) = ca * cb; + + return rm; +} + + +int main_pyTsai() +{ + + double image_w = 1024; + double image_h = 1024; + // 鍐呭弬 + double f = 1011; + double dx = 0.209, dy = 0.209; + double u0 = 505, v0 = 509.1; + + // 澶栧弬 + double rx = 0.001; + double ry = -0.001; + double rz = 0.002; + + double tx = 10; + double ty = -10; + double tz = 950; + + // 鐣稿彉鍙傛暟 + double k1 = 1; + double k2 = 100; + double p1 = -0.01; + double p2 = -0.01; + double s1 = -0.02; + double s3 = 0.01; + + + cv::Mat camera_matrix = (cv::Mat_(3, 3) << f / dx, 0, u0, 0, f / dy, v0, 0, 0, 1); + cv::Mat camera_matrix_undistortion = (cv::Mat_(3, 3) << f / dx, 0, (image_w - 1.0) / 2, 0, f / dy, (image_h - 1.0) / 2, 0, 0, 1); + cv::Mat rotation = angle2mat(rx, ry, rz); + cv::Mat rvec = (cv::Mat_(3, 1) << rz, ry, rz); + cv::Mat translation = (cv::Mat_(3, 1) << tx, ty, tz); + + // k1,k2,p1,p2,k3,k4,k5,k6,s1,s2,s3,s4 + cv::Mat distortion_coeff = (cv::Mat_(12, 1) << k1, k2, p1, p2, 0, 0, 0, 0, s1, 0, s3, 0); + + std::vector points3D = + { +#include "marker3d.data" + }; + + std::vector points2D; + cv::projectPoints(points3D, rvec, translation, camera_matrix, distortion_coeff, points2D); + + tsai_cd.point_count = points3D.size(); + for (size_t i = 0; i < tsai_cd.point_count; i++) + { + tsai_cd.xw[i] = points3D[i][0]; + tsai_cd.yw[i] = points3D[i][1]; + tsai_cd.zw[i] = points3D[i][2]; + tsai_cd.Xf[i] = points2D[i][0]; + tsai_cd.Yf[i] = points2D[i][1]; + } + + + + + + + // Initial camera parameters + tsai_cp.Ncx = 1024; + tsai_cp.Ncy = 1024; + tsai_cp.dx = 0.209; + tsai_cp.dy = 0.209; + tsai_cp.Cx = (tsai_cp.Ncx - 1.0) / 2.0; + tsai_cp.Cy = (tsai_cp.Ncy - 1.0) / 2.0; + tsai_cp.sx = 1.0; + + //set_data(); + + /* + if (!noncoplanar_calibration_with_full_optimization()) + { + printf("Errors when Tsai Calibration! Please have a check!\n"); + }*/ + + + /* start with a 3 parameter (Tz, f, kappa1) optimization */ + if (!ncc_three_parm_optimization()) + return 0; + printf("==============Only Tz,f,k1 optimization==============\n"); + print_parameters(&tsai_cp); + print_constants(&tsai_cc); + print_errors(); + show_errors("Only Tz,f,k1 optimization"); + + //set_data(); + + /* do a full optimization minus the image center */ + if (!ncc_nic_optimization()) + return 0; + + printf("==============optimization minus the image center==============\n"); + print_parameters(&tsai_cp); + print_constants(&tsai_cc); + print_errors(); + show_errors("optimization minus the image center"); + + /* do a full optimization including the image center */ + if (!ncc_full_optimization()) + return 0; + + printf("==============full optimization==============\n"); + print_parameters(&tsai_cp); + print_constants(&tsai_cc); + print_errors(); + show_errors("full optimization"); + + //print_parameters(&tsai_cp); + //print_constants(&tsai_cc); + //print_errors(); + + + return 0; +} \ No newline at end of file diff --git a/others/pyTsai/setup.py b/others/pyTsai/setup.py new file mode 100644 index 0000000..667ffe2 --- /dev/null +++ b/others/pyTsai/setup.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python + +from distutils.core import * + +pytsai_ext = Extension( + 'pytsai', [ + 'src/pytsai.c', + 'src/errors.c', + 'src/tsai/cal_eval.c', + 'src/tsai/cal_main.c', + 'src/tsai/cal_tran.c', + 'src/tsai/ecalmain.c', + 'src/minpack/dpmpar.c', + 'src/minpack/enorm.c', + 'src/minpack/fdjac2.c', + 'src/minpack/lmdif.c', + 'src/minpack/lmpar.c', + 'src/minpack/qrfac.c', + 'src/minpack/qrsolv.c', + 'src/matrix/matrix.c' +]) +#extra_compile_args=['-O2', '-Wall', '-pedantic', '-std=c99', +#'-W', '-Wunreachable-code']) + +setup( + name = 'PyTsai', + version = '1.0', + description = 'Tsai camera calibration.', + author = 'Jonathan Merritt, Gabor Cseh', + author_email = 'j.merritt@pgrad.unimelb.edu.au, csega@mailbox.hu', + keywords = "tsai automatic calibration python blender", + url = "https://github.com/Csega/pyTsai", # project home page, if any + ext_modules = [ pytsai_ext ], + py_modules = [ 'Tsai' ] +) diff --git a/others/pyTsai/src/errors.c b/others/pyTsai/src/errors.c new file mode 100644 index 0000000..94ad0ee --- /dev/null +++ b/others/pyTsai/src/errors.c @@ -0,0 +1,74 @@ +/** + * errors.c + * Copyright (C) Jonathan Merritt 2004. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + + /***************************************************************************\ + * Methods for handling errors. * + * * + * The approach taken for setting up the Tsai camera matching code as a * + * Python extension module is as follows. The library originally used a * + * combination of sprintf() and exit() to indicate errors and exit: * + * sprintf(stderr, "some error\n"); * + * exit(-1); * + * This has been replaced by setting of an error flag, and the functions * + * that can fail now return 0 on failure and 1 on success rather than being * + * of void return type as before: * + * pytsai_raise("some error"); * + * return 0; * + * * + * To trap errors for the Python wrapper, the following is the template: * + * pytsai_clear(); * + * --- perform C-library calls here --- * + * if (pytsai_haserror()) * + * --- raise Python exception --- * + \***************************************************************************/ + +#include +#include "errors.h" + +/* true / false error flag */ +int pytsai_error; +/* character array to store an error string */ +char pytsai_string[ERROR_BUFFER_SIZE]; + +/** + * Clears the error flag. + */ +void pytsai_clear() +{ + pytsai_error = 0; +} + +/** + * Sets the error flag and stores an error message. + */ +void pytsai_raise(char *message) +{ + pytsai_error = 1; + strncpy(pytsai_string, message, ERROR_BUFFER_SIZE-1); +} + +/** + * Returns non-zero if the error flag has been set. + */ +int pytsai_haserror() +{ + return pytsai_error; +} + diff --git a/others/pyTsai/src/errors.h b/others/pyTsai/src/errors.h new file mode 100644 index 0000000..e6d4c3d --- /dev/null +++ b/others/pyTsai/src/errors.h @@ -0,0 +1,39 @@ +/** + * errors.h + * Copyright (C) Jonathan Merritt 2004. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/* See the file errors.c for a description. */ + +#ifndef ERRORS_H +#define ERRORS_H + +/* Size of the buffer containing any error string. */ +#define ERROR_BUFFER_SIZE 1024 + +/* Storage of error quantities. */ +extern int pytsai_error; /* true / false error flag */ +extern char pytsai_string[]; /* error description string */ + +/* Error methods. */ +void pytsai_clear(); +void pytsai_raise(char *message); +int pytsai_haserror(); + +#endif /* ERRORS_H */ + diff --git a/others/pyTsai/src/matrix/.sconsign b/others/pyTsai/src/matrix/.sconsign new file mode 100644 index 0000000..bc369ca --- /dev/null +++ b/others/pyTsai/src/matrix/.sconsign @@ -0,0 +1,3 @@ +}qUmatrix.hq(cSCons.Node.FS +BuildInfo +qoq}q(U timestampqJ奤0UcsigqU 46d18abe1db1ca76b1d5d40d6aa2c223qubs. \ No newline at end of file diff --git a/others/pyTsai/src/matrix/matrix.c b/others/pyTsai/src/matrix/matrix.c new file mode 100644 index 0000000..376886c --- /dev/null +++ b/others/pyTsai/src/matrix/matrix.c @@ -0,0 +1,499 @@ +/* matrix.c -- library routines for constructing dynamic matrices with + * arbitrary bounds using Iliffe vectors + **************************************************************** + * HISTORY + * + * 15-Jul-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN + * Use stdlib.h rather than defining calloc locally - this fixes a + * weird problem with Borland 4.5 C. + * + * 02-Apr-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN + * Rewrite memory allocation to avoid memory alignment problems + * on some machines. + * + * 25-Nov-80 David Smith (drs) at Carnegie-Mellon University + * Changed virtual base address name to "el" for all data + * types (Previously vali, vald, ...) This was possible due to the + * compiler enhancement which keeps different structure declarations + * separate. + * + * 30-Oct-80 David Smith (drs) at Carnegie-Mellon University + * Rewritten for record-style matrices + * + * 28-Oct-80 David Smith (drs) at Carnegie-Mellon University + * Written. + * */ + +#include +#include +#include +#include +#include "matrix.h" + +#define FALSE 0 +#define TRUE 1 + + +/* + Print an Iliffe matrix out to stdout. +*/ +void print_mat (mat) + dmat mat; +{ + int i, + j; + + fprintf (stdout, " "); + for (i = mat.lb2; i <= mat.ub2; i++) + fprintf (stdout, " %7d", i); + fprintf (stdout, "\n"); + + for (j = mat.lb1; j <= mat.ub1; j++) { + fprintf (stdout, " %7d", j); + for (i = mat.lb2; i <= mat.ub2; i++) + fprintf (stdout, " %7.2lf", mat.el[j][i]); + fprintf (stdout, "\n"); + } +} + + +/* + Allocates and initializes memory for a double precision Iliffe matrix. +*/ +dmat newdmat (rs, re, cs, ce, error) + int rs, + re, + cs, + ce, + *error; +{ + double *p, + **b; + + int r, + rows, + cols; + + dmat matrix; + + rows = re - rs + 1; + cols = ce - cs + 1; + if (rows <= 0 || cols <= 0) { + errno = EDOM; + *error = -1; + return (matrix); + } + + /* fill in the bounds for this matrix */ + matrix.lb1 = rs; + matrix.ub1 = re; + matrix.lb2 = cs; + matrix.ub2 = ce; + + /* allocate memory for the row pointers */ + b = (double **) malloc ((unsigned int) rows * (unsigned int) sizeof (double *)); + if (b == 0) { + errno = ENOMEM; + *error = -1; + return (matrix); + } + + /* adjust for non-zero lower index bounds */ + matrix.el = b -= rs; + + /* allocate memory for storing the actual matrix */ + p = (double *) malloc ((unsigned int) rows * cols * (unsigned int) sizeof (double)); + if (p == 0) { + errno = ENOMEM; + *error = -1; + return (matrix); + } + + /* keep a reminder where the block is actually located */ + matrix.mat_sto = (char *) p; + + /* adjust for non-zero lower index bounds */ + p -= cs; + + /* fabricate row pointers into the matrix */ + for (r = rs; r <= re; r++) { + b[r] = p; + p += cols; + } + + *error = 0; + return (matrix); +} + + +/* + Perform the matrix multiplication c = a*b where + a, b, and c are dynamic Iliffe matrices. + + The matrix dimensions must be commensurate. This means that + c and a must have the same number of rows; c and b must have + the same number of columns; and a must have the same number + of columns as b has rows. The actual index origins are + immaterial. + + If c is the same matrix as a or b, the result will be + correct at the expense of more time. +*/ +int matmul (a, b, c) + dmat a, + b, + c; +{ + int i, + j, + k, + broff, + croff, + ccoff, /* coordinate origin offsets */ + mem_alloced, + error; + + double t; + + dmat d; /* temporary workspace matrix */ + + if (a.ub2 - a.lb2 != b.ub1 - b.lb1 + || a.ub1 - a.lb1 != c.ub1 - c.lb1 + || b.ub2 - b.lb2 != c.ub2 - c.lb2) { + errno = EDOM; + return (-1); + } + + if (a.mat_sto != c.mat_sto && b.mat_sto != c.mat_sto) { + d = c; + mem_alloced = FALSE; + } else { + d = newdmat (c.lb1, c.ub1, c.lb2, c.ub2, &error); + if (error) { + fprintf (stderr, "Matmul: out of storage.\n"); + errno = ENOMEM; + return (-1); + } + mem_alloced = TRUE; + } + + broff = b.lb1 - a.lb2; /* B's row offset from A */ + croff = c.lb1 - a.lb1; /* C's row offset from A */ + ccoff = c.lb2 - b.lb2; /* C's column offset from B */ + for (i = a.lb1; i <= a.ub1; i++) + for (j = b.lb2; j <= b.ub2; j++) { + t = 0.0; + for (k = a.lb2; k <= a.ub2; k++) + t += a.el[i][k] * b.el[k + broff][j]; + d.el[i + croff][j + ccoff] = t; + } + + if (mem_alloced) { + for (i = c.lb1; i <= c.ub1; i++) + for (j = c.lb2; j <= c.ub2; j++) + c.el[i][j] = d.el[i][j]; + freemat (d); + } + + return (0); +} + + +/* + Copy dynamic Iliffe matrix A into RSLT. + + Bounds need not be the same but the dimensions must be. +*/ +int matcopy (A, RSLT) + dmat A, + RSLT; +{ + int i, + j, + rowsize, + colsize; + + double **a = A.el, + **rslt = RSLT.el; + + rowsize = A.ub1 - A.lb1; + colsize = A.ub2 - A.lb2; + if (rowsize != RSLT.ub1 - RSLT.lb1 || colsize != RSLT.ub2 - RSLT.lb2) { + errno = EDOM; + return (-1); + } + + for (i = 0; i <= rowsize; i++) + for (j = 0; j <= colsize; j++) + rslt[RSLT.lb1 + i][RSLT.lb2 + j] = a[A.lb1 + i][A.lb2 + j]; + + return (0); +} + + +/* + Generate the transpose of a dynamic Iliffe matrix. +*/ +int transpose (A, ATrans) + dmat A, + ATrans; +{ + int i, + j, + rowsize, + colsize, + error; + + double **a = A.el, + **atrans = ATrans.el, + temp; + + dmat TMP; + + rowsize = A.ub1 - A.lb1; + colsize = A.ub2 - A.lb2; + if (rowsize < 0 || rowsize != ATrans.ub2 - ATrans.lb2 || + colsize < 0 || colsize != ATrans.ub1 - ATrans.lb1) { + errno = EDOM; + return (-1); + } + + if (A.mat_sto == ATrans.mat_sto + && A.lb1 == ATrans.lb1 + && A.lb2 == ATrans.lb2) { + for (i = 0; i <= rowsize; i++) + for (j = i + 1; j <= colsize; j++) { + temp = a[A.lb1 + i][A.lb2 + j]; + atrans[A.lb1 + i][A.lb2 + j] = a[A.lb1 + j][A.lb2 + i]; + atrans[A.lb1 + j][A.lb2 + i] = temp; + } + } else if (A.mat_sto == ATrans.mat_sto) { + TMP = newdmat (ATrans.lb1, ATrans.ub1, ATrans.lb2, ATrans.ub2, &error); + if (error) + return (-2); + + for (i = 0; i <= rowsize; i++) + for (j = 0; j <= colsize; j++) { + TMP.el[ATrans.lb1 + j][ATrans.lb2 + i] = + a[A.lb1 + i][A.lb2 + j]; + } + + matcopy (TMP, ATrans); + freemat (TMP); + } else { + for (i = 0; i <= rowsize; i++) + for (j = 0; j <= colsize; j++) + atrans[ATrans.lb1 + j][ATrans.lb2 + i] + = a[A.lb1 + i][A.lb2 + j]; + } + + return (0); +} + + +/* + In-place Iliffe matrix inversion using full pivoting. + The standard Gauss-Jordan method is used. + The return value is the determinant. +*/ + +#define PERMBUFSIZE 100 /* Mat bigger than this requires calling calloc. */ + +double matinvert (a) + dmat a; +{ + int i, + j, + k, + *l, + *m, + permbuf[2 * PERMBUFSIZE], + mem_alloced; + + double det, + biga, + hold; + + if (a.lb1 != a.lb2 || a.ub1 != a.ub2) { + errno = EDOM; + return (0.0); + } + + /* Allocate permutation vectors for l and m, with the same origin as the matrix. */ + if (a.ub1 - a.lb1 + 1 <= PERMBUFSIZE) { + l = permbuf; + mem_alloced = FALSE; + } else { + l = (int *) malloc ((unsigned int) 2 * (a.ub1 - a.lb1 + 1) * (unsigned int) sizeof (int)); + if (l == 0) { + fprintf (stderr, "matinvert: can't get working storage.\n"); + errno = ENOMEM; + return (0.0); + } + mem_alloced = TRUE; + } + + l -= a.lb1; + m = l + (a.ub1 - a.lb1 + 1); + + det = 1.0; + for (k = a.lb1; k <= a.ub1; k++) { + l[k] = k; + m[k] = k; + biga = a.el[k][k]; + + /* Find the biggest element in the submatrix */ + for (i = k; i <= a.ub1; i++) + for (j = k; j <= a.ub2; j++) + if (fabs (a.el[i][j]) > fabs (biga)) { + biga = a.el[i][j]; + l[k] = i; + m[k] = j; + } + + /* Interchange rows */ + i = l[k]; + if (i > k) + for (j = a.lb2; j <= a.ub2; j++) { + hold = -a.el[k][j]; + a.el[k][j] = a.el[i][j]; + a.el[i][j] = hold; + } + + /* Interchange columns */ + j = m[k]; + if (j > k) + for (i = a.lb1; i <= a.ub1; i++) { + hold = -a.el[i][k]; + a.el[i][k] = a.el[i][j]; + a.el[i][j] = hold; + } + + /* Divide column by minus pivot (value of pivot element is contained in biga). */ + if (biga == 0.0) + return (0.0); + + for (i = a.lb1; i <= a.ub1; i++) + if (i != k) + a.el[i][k] /= -biga; + + /* Reduce matrix */ + for (i = a.lb1; i <= a.ub1; i++) + if (i != k) { + hold = a.el[i][k]; + for (j = a.lb2; j <= a.ub2; j++) + if (j != k) + a.el[i][j] += hold * a.el[k][j]; + } + + /* Divide row by pivot */ + for (j = a.lb2; j <= a.ub2; j++) + if (j != k) + a.el[k][j] /= biga; + + det *= biga; /* Product of pivots */ + a.el[k][k] = 1.0 / biga; + } /* K loop */ + + /* Final row & column interchanges */ + for (k = a.ub1 - 1; k >= a.lb1; k--) { + i = l[k]; + if (i > k) + for (j = a.lb2; j <= a.ub2; j++) { + hold = a.el[j][k]; + a.el[j][k] = -a.el[j][i]; + a.el[j][i] = hold; + } + + j = m[k]; + if (j > k) + for (i = a.lb1; i <= a.ub1; i++) { + hold = a.el[k][i]; + a.el[k][i] = -a.el[j][i]; + a.el[j][i] = hold; + } + } + + if (mem_alloced) + free (l + a.lb1); + + return det; +} + + +/* + Solve the overconstrained linear system Ma = b using a least + squares error (pseudo inverse) approach. +*/ +int solve_system (M, a, b) + dmat M, + a, + b; +{ + dmat Mt, + MtM, + Mdag; + + if ((M.ub1 - M.lb1) < (M.ub2 - M.lb2)) { + fprintf (stderr, "solve_system: matrix M has more columns than rows\n"); + return (-1); + } + + Mt = newdmat (M.lb2, M.ub2, M.lb1, M.ub1, &errno); + if (errno) { + fprintf (stderr, "solve_system: unable to allocate matrix M_transpose\n"); + return (-1); + } + + transpose (M, Mt); + if (errno) { + fprintf (stderr, "solve_system: unable to transpose matrix M\n"); + return (-1); + } + + MtM = newdmat (M.lb2, M.ub2, M.lb2, M.ub2, &errno); + if (errno) { + fprintf (stderr, "solve_system: unable to allocate matrix M_transpose_M\n"); + return (-1); + } + + matmul (Mt, M, MtM); + if (errno) { + fprintf (stderr, "solve_system: unable to compute matrix product of M_transpose and M\n"); + return (-1); + } + + if (fabs (matinvert (MtM)) < 0.001) { + fprintf (stderr, "solve_system: determinant of matrix M_transpose_M is too small\n"); + return (-1); + } + + if (errno) { + fprintf (stderr, "solve_system: error during matrix inversion\n"); + return (-1); + } + + Mdag = newdmat (M.lb2, M.ub2, M.lb1, M.ub1, &errno); + if (errno) { + fprintf (stderr, "solve_system: unable to allocate matrix M_diag\n"); + return (-1); + } + + matmul (MtM, Mt, Mdag); + if (errno) { + fprintf (stderr, "solve_system: unable to compute matrix product of M_transpose_M and M_transpose\n"); + return (-1); + } + + matmul (Mdag, b, a); + if (errno) { + fprintf (stderr, "solve_system: unable to compute matrix product of M_diag and b\n"); + return (-1); + } + + freemat (Mt); + freemat (MtM); + freemat (Mdag); + + return 0; +} diff --git a/others/pyTsai/src/matrix/matrix.h b/others/pyTsai/src/matrix/matrix.h new file mode 100644 index 0000000..44e6152 --- /dev/null +++ b/others/pyTsai/src/matrix/matrix.h @@ -0,0 +1,33 @@ +/* matrix.h -- define types for matrices using Iliffe vectors + * + ************************************************************* + * HISTORY + * + * 02-Apr-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN + * Rewrite memory allocation to avoid memory alignment problems + * on some machines. + */ + +#ifndef MATRIX_H +#define MATRIX_H + +typedef struct { + int lb1, + ub1, + lb2, + ub2; + char *mat_sto; + double **el; +} dmat; + +void print_mat (); +dmat newdmat (); +int matmul (); +int matcopy (); +int transpose (); +double matinvert (); +int solve_system (); + +#define freemat(m) free((m).mat_sto) ; free((m).el) + +#endif /* MATRIX_H */ diff --git a/others/pyTsai/src/minpack/.sconsign b/others/pyTsai/src/minpack/.sconsign new file mode 100644 index 0000000..c64ee3b --- /dev/null +++ b/others/pyTsai/src/minpack/.sconsign @@ -0,0 +1,12 @@ +}q(Ulmpar.oq(cSCons.Node.FS +BuildInfo +qoq}q(UbsigqU f494ae4bd10f03b760413f33ed8a2920qU bimplicitq]q U minpack/f2c.hq +aUbdependsq ]q U bdependsigsq ]qUbactsigqU f10f37ad209f5b6bc80919394e8b072fqU bimplicitsigsq]qU 20317d08b7d4eca2e0ec0490ce5ed984qaUbactqUgcc -c -o lmpar.o lmpar.cqUbsourcesq]qUminpack/lmpar.cqaU bsourcesigsq]qU 1ce6582c54d61f729766cf6f8d387835qaubUenorm.oq(hoq}q(hU 336c3deb2ee595ec6fea1f5f67e603a4qh]q h +ah ]q!h ]q"hU 94efc941d5e90c50f40e0333e0c0babeq#h]q$hahUgcc -c -o enorm.o enorm.cq%h]q&Uminpack/enorm.cq'ah]q(U dad412c21ca42aca012eaaadf050aa94q)aubUfdjac2.cq*(hoq+}q,(U timestampq-Jx|/Ucsigq.U 851d2fb9e0ebd4ff43758db7f9047fceq/ubUlmdif.cq0(hoq1}q2(h-Jx|/h.U 7607edfbb71af7f7b3096d5a63a8b664q3ubU +libmnpak.aq4(hoq5}q6(Ubsigq7U 24da5a516265867398e40369cdb84e27q8U bimplicitq9]q:Ubdependsq;]qUbactsigq?U 745b3f6bd0ef599834e901d1bea7865aq@U bimplicitsigsqA]qBUbactqCU[ar r libmnpak.a dpmpar.o enorm.o fdjac2.o lmdif.o lmpar.o qrfac.o qrsolv.oranlib libmnpak.aqDUbsourcesqE]qF(Uminpack/dpmpar.oqGUminpack/enorm.oqHUminpack/fdjac2.oqIUminpack/lmdif.oqJUminpack/lmpar.oqKUminpack/qrfac.oqLUminpack/qrsolv.oqMeU bsourcesigsqN]qO(U 1dbda9de104bd3716d0783cb19a85d26qPhU 168a1e77af6453c365480570109a6552qQU 905d7ea6f5f7e873e5c0b2c005ecc4b1qRhU 10fa47b5a32800d28c6d21e905a2d06cqSU dfcf614da708c31648e77eb7551cfcd3qTeubUlmdif.oqU(hoqV}qW(hhRh]qXh +ah ]qYh ]qZhU e4db6e5f37d4694d4e2f0c6af1b0b90eq[h]q\hahUgcc -c -o lmdif.o lmdif.cq]h]q^Uminpack/lmdif.cq_ah]q`h3aubUfdjac2.oqa(hoqb}qc(hhQh]qdh +ah ]qeh ]qfhU 4e938476de7e0c94cb4ce1f21d9361ffqgh]qhhahUgcc -c -o fdjac2.o fdjac2.cqih]qjUminpack/fdjac2.cqkah]qlh/aubUlmpar.cqm(hoqn}qo(h-J +x|/h.hubUenorm.cqp(hoqq}qr(h-Jx|/h.h)ubUqrsolv.cqs(hoqt}qu(h-Jx|/h.U fd8aeb68901441a4f4da789ffa42071eqvubUdpmpar.cqw(hoqx}qy(h-Jx|/h.U aecfcaa3c93b839f98501ca890925b81qzubUqrfac.oq{(hoq|}q}(hhSh]q~h +ah ]qh ]qhU ad98ac8aa0a3e582e7e510e68bad82baq乭]q俬ahUgcc -c -o qrfac.o qrfac.cq僪]q刄minpack/qrfac.cq卆h]q哢 5fe219f8642c85dde13354062459a843q嘺ubUf2c.hq(hoq墋q(h-J|)h.hubUqrsolv.oq(hoq寎q(hhTh]q巋 +ah ]q廻 ]q恏U 35ec393beb8fd2d9a39585c5423dde1fq慼]q抙ahUgcc -c -o qrsolv.o qrsolv.cq揾]q擴minpack/qrsolv.cq昦h]q杊vaubUqrfac.cq(hoq榼q(h-J x|/h.h噓bUdpmpar.oq(hoq泒q(hhPh]q漢 +ah ]q瀐 ]q焗U 9071f0c8ac7d7e4864e73cdc1cd069dbq爃]qahUgcc -c -o dpmpar.o dpmpar.cq]qminpack/dpmpar.cqh]qzaubu. \ No newline at end of file diff --git a/others/pyTsai/src/minpack/dpmpar.c b/others/pyTsai/src/minpack/dpmpar.c new file mode 100644 index 0000000..b8964cd --- /dev/null +++ b/others/pyTsai/src/minpack/dpmpar.c @@ -0,0 +1,203 @@ +/* dpmpar.f -- translated by f2c (version of 17 January 1992 0:17:58). + You must link the resulting object file with the libraries: + -lf77 -li77 -lm -lc (in that order) +*/ + +#include "f2c.h" + +doublereal dpmpar_(i) +integer *i; +{ + /* Initialized data */ + + static struct { + integer e_1[6]; + doublereal fill_2[1]; + doublereal e_3; + } equiv_2 = { {1018167296, 0, 1048576, 0, 2146435071, -1}, {0}, 0. }; + + + /* System generated locals */ + doublereal ret_val; + + /* Local variables */ +#define dmach ((doublereal *)&equiv_2) +#define minmag ((integer *)&equiv_2 + 2) +#define maxmag ((integer *)&equiv_2 + 4) +#define mcheps ((integer *)&equiv_2) + +/* ********** */ + +/* function dpmpar */ + +/* This function provides double precision machine parameters */ +/* when the appropriate set of data statements is activated (by */ +/* removing the c from column 1) and all other data statements are */ +/* rendered inactive. Most of the parameter values were obtained */ +/* from the corresponding Bell Laboratories Port Library function. */ + +/* The function statement is */ + +/* double precision function dpmpar(i) */ + +/* where */ + +/* i is an integer input variable set to 1, 2, or 3 which */ +/* selects the desired machine parameter. If the machine has */ +/* t base b digits and its smallest and largest exponents are */ +/* emin and emax, respectively, then these parameters are */ + +/* dpmpar(1) = b**(1 - t), the machine precision, */ + +/* dpmpar(2) = b**(emin - 1), the smallest magnitude, */ + +/* dpmpar(3) = b**emax*(1 - b**(-t)), the largest magnitude. */ + +/* Argonne National Laboratory. MINPACK Project. June 1983. */ +/* Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More */ + +/* ********** */ + +/* Machine constants for the IBM 360/370 series, */ +/* the Amdahl 470/V6, the ICL 2900, the Itel AS/6, */ +/* the Xerox Sigma 5/7/9 and the Sel systems 85/86. */ + +/* data mcheps(1),mcheps(2) / z34100000, z00000000 / */ +/* data minmag(1),minmag(2) / z00100000, z00000000 / */ +/* data maxmag(1),maxmag(2) / z7fffffff, zffffffff / */ + +/* Machine constants for the Honeywell 600/6000 series. */ + +/* data mcheps(1),mcheps(2) / o606400000000, o000000000000 / */ +/* data minmag(1),minmag(2) / o402400000000, o000000000000 / */ +/* data maxmag(1),maxmag(2) / o376777777777, o777777777777 / */ + +/* Machine constants for the CDC 6000/7000 series. */ + +/* data mcheps(1) / 15614000000000000000b / */ +/* data mcheps(2) / 15010000000000000000b / */ + +/* data minmag(1) / 00604000000000000000b / */ +/* data minmag(2) / 00000000000000000000b / */ + +/* data maxmag(1) / 37767777777777777777b / */ +/* data maxmag(2) / 37167777777777777777b / */ + +/* Machine constants for the PDP-10 (KA processor). */ + +/* data mcheps(1),mcheps(2) / "114400000000, "000000000000 / */ +/* data minmag(1),minmag(2) / "033400000000, "000000000000 / */ +/* data maxmag(1),maxmag(2) / "377777777777, "344777777777 / */ + +/* Machine constants for the PDP-10 (KI processor). */ + +/* data mcheps(1),mcheps(2) / "104400000000, "000000000000 / */ +/* data minmag(1),minmag(2) / "000400000000, "000000000000 / */ +/* data maxmag(1),maxmag(2) / "377777777777, "377777777777 / */ + +/* Machine constants for the PDP-11. */ + +/* data mcheps(1),mcheps(2) / 9472, 0 / */ +/* data mcheps(3),mcheps(4) / 0, 0 / */ + +/* data minmag(1),minmag(2) / 128, 0 / */ +/* data minmag(3),minmag(4) / 0, 0 / */ + +/* data maxmag(1),maxmag(2) / 32767, -1 / */ +/* data maxmag(3),maxmag(4) / -1, -1 / */ + +/* Machine constants for the Burroughs 6700/7700 systems. */ + +/* data mcheps(1) / o1451000000000000 / */ +/* data mcheps(2) / o0000000000000000 / */ + +/* data minmag(1) / o1771000000000000 / */ +/* data minmag(2) / o7770000000000000 / */ + +/* data maxmag(1) / o0777777777777777 / */ +/* data maxmag(2) / o7777777777777777 / */ + +/* Machine constants for the Burroughs 5700 system. */ + +/* data mcheps(1) / o1451000000000000 / */ +/* data mcheps(2) / o0000000000000000 / */ + +/* data minmag(1) / o1771000000000000 / */ +/* data minmag(2) / o0000000000000000 / */ + +/* data maxmag(1) / o0777777777777777 / */ +/* data maxmag(2) / o0007777777777777 / */ + +/* Machine constants for the Burroughs 1700 system. */ + +/* data mcheps(1) / zcc6800000 / */ +/* data mcheps(2) / z000000000 / */ + +/* data minmag(1) / zc00800000 / */ +/* data minmag(2) / z000000000 / */ + +/* data maxmag(1) / zdffffffff / */ +/* data maxmag(2) / zfffffffff / */ + +/* Machine constants for the Univac 1100 series. */ + +/* data mcheps(1),mcheps(2) / o170640000000, o000000000000 / */ +/* data minmag(1),minmag(2) / o000040000000, o000000000000 / */ +/* data maxmag(1),maxmag(2) / o377777777777, o777777777777 / */ + +/* Machine constants for the Data General Eclipse S/200. */ + +/* Note - it may be appropriate to include the following card - */ +/* static dmach(3) */ + +/* data minmag/20k,3*0/,maxmag/77777k,3*177777k/ */ +/* data mcheps/32020k,3*0/ */ + +/* Machine constants for the Harris 220. */ + +/* data mcheps(1),mcheps(2) / '20000000, '00000334 / */ +/* data minmag(1),minmag(2) / '20000000, '00000201 / */ +/* data maxmag(1),maxmag(2) / '37777777, '37777577 / */ + +/* Machine constants for the Cray-1. */ + +/* data mcheps(1) / 0376424000000000000000b / */ +/* data mcheps(2) / 0000000000000000000000b / */ + +/* data minmag(1) / 0200034000000000000000b / */ +/* data minmag(2) / 0000000000000000000000b / */ + +/* data maxmag(1) / 0577777777777777777777b / */ +/* data maxmag(2) / 0000007777777777777776b / */ + +/* Machine constants for the Prime 400. */ + +/* data mcheps(1),mcheps(2) / :10000000000, :00000000123 / */ +/* data minmag(1),minmag(2) / :10000000000, :00000100000 / */ +/* data maxmag(1),maxmag(2) / :17777777777, :37777677776 / */ + +/* Machine constants for the VAX-11. */ + +/* data mcheps(1),mcheps(2) / 9472, 0 / */ +/* data minmag(1),minmag(2) / 128, 0 / */ +/* data maxmag(1),maxmag(2) / -32769, -1 / */ + +/* Machine constants for the IEEE double precision floating point. */ +/* taken from values of dmach(4), dmach(1) and dmach(2) in d1mach.f, +*/ +/* generated by machar.f from the BLAS distribution in Netlib. */ + + + ret_val = dmach[*i - 1]; + return ret_val; + +/* Last card of function dpmpar. */ + +} /* dpmpar_ */ + +#undef mcheps +#undef maxmag +#undef minmag +#undef dmach + + diff --git a/others/pyTsai/src/minpack/dpmpar.f b/others/pyTsai/src/minpack/dpmpar.f new file mode 100644 index 0000000..5c1934e --- /dev/null +++ b/others/pyTsai/src/minpack/dpmpar.f @@ -0,0 +1,179 @@ + double precision function dpmpar(i) + integer i +c ********** +c +c function dpmpar +c +c This function provides double precision machine parameters +c when the appropriate set of data statements is activated (by +c removing the c from column 1) and all other data statements are +c rendered inactive. Most of the parameter values were obtained +c from the corresponding Bell Laboratories Port Library function. +c +c The function statement is +c +c double precision function dpmpar(i) +c +c where +c +c i is an integer input variable set to 1, 2, or 3 which +c selects the desired machine parameter. If the machine has +c t base b digits and its smallest and largest exponents are +c emin and emax, respectively, then these parameters are +c +c dpmpar(1) = b**(1 - t), the machine precision, +c +c dpmpar(2) = b**(emin - 1), the smallest magnitude, +c +c dpmpar(3) = b**emax*(1 - b**(-t)), the largest magnitude. +c +c Argonne National Laboratory. MINPACK Project. June 1983. +c Burton S. Garbow, Kenneth E. Hillstrom, Jorge J. More +c +c ********** + integer mcheps(4) + integer minmag(4) + integer maxmag(4) + double precision dmach(3) + equivalence (dmach(1),mcheps(1)) + equivalence (dmach(2),minmag(1)) + equivalence (dmach(3),maxmag(1)) +c +c Machine constants for the IBM 360/370 series, +c the Amdahl 470/V6, the ICL 2900, the Itel AS/6, +c the Xerox Sigma 5/7/9 and the Sel systems 85/86. +c +c data mcheps(1),mcheps(2) / z34100000, z00000000 / +c data minmag(1),minmag(2) / z00100000, z00000000 / +c data maxmag(1),maxmag(2) / z7fffffff, zffffffff / +c +c Machine constants for the Honeywell 600/6000 series. +c +c data mcheps(1),mcheps(2) / o606400000000, o000000000000 / +c data minmag(1),minmag(2) / o402400000000, o000000000000 / +c data maxmag(1),maxmag(2) / o376777777777, o777777777777 / +c +c Machine constants for the CDC 6000/7000 series. +c +c data mcheps(1) / 15614000000000000000b / +c data mcheps(2) / 15010000000000000000b / +c +c data minmag(1) / 00604000000000000000b / +c data minmag(2) / 00000000000000000000b / +c +c data maxmag(1) / 37767777777777777777b / +c data maxmag(2) / 37167777777777777777b / +c +c Machine constants for the PDP-10 (KA processor). +c +c data mcheps(1),mcheps(2) / "114400000000, "000000000000 / +c data minmag(1),minmag(2) / "033400000000, "000000000000 / +c data maxmag(1),maxmag(2) / "377777777777, "344777777777 / +c +c Machine constants for the PDP-10 (KI processor). +c +c data mcheps(1),mcheps(2) / "104400000000, "000000000000 / +c data minmag(1),minmag(2) / "000400000000, "000000000000 / +c data maxmag(1),maxmag(2) / "377777777777, "377777777777 / +c +c Machine constants for the PDP-11. +c +c data mcheps(1),mcheps(2) / 9472, 0 / +c data mcheps(3),mcheps(4) / 0, 0 / +c +c data minmag(1),minmag(2) / 128, 0 / +c data minmag(3),minmag(4) / 0, 0 / +c +c data maxmag(1),maxmag(2) / 32767, -1 / +c data maxmag(3),maxmag(4) / -1, -1 / +c +c Machine constants for the Burroughs 6700/7700 systems. +c +c data mcheps(1) / o1451000000000000 / +c data mcheps(2) / o0000000000000000 / +c +c data minmag(1) / o1771000000000000 / +c data minmag(2) / o7770000000000000 / +c +c data maxmag(1) / o0777777777777777 / +c data maxmag(2) / o7777777777777777 / +c +c Machine constants for the Burroughs 5700 system. +c +c data mcheps(1) / o1451000000000000 / +c data mcheps(2) / o0000000000000000 / +c +c data minmag(1) / o1771000000000000 / +c data minmag(2) / o0000000000000000 / +c +c data maxmag(1) / o0777777777777777 / +c data maxmag(2) / o0007777777777777 / +c +c Machine constants for the Burroughs 1700 system. +c +c data mcheps(1) / zcc6800000 / +c data mcheps(2) / z000000000 / +c +c data minmag(1) / zc00800000 / +c data minmag(2) / z000000000 / +c +c data maxmag(1) / zdffffffff / +c data maxmag(2) / zfffffffff / +c +c Machine constants for the Univac 1100 series. +c +c data mcheps(1),mcheps(2) / o170640000000, o000000000000 / +c data minmag(1),minmag(2) / o000040000000, o000000000000 / +c data maxmag(1),maxmag(2) / o377777777777, o777777777777 / +c +c Machine constants for the Data General Eclipse S/200. +c +c Note - it may be appropriate to include the following card - +c static dmach(3) +c +c data minmag/20k,3*0/,maxmag/77777k,3*177777k/ +c data mcheps/32020k,3*0/ +c +c Machine constants for the Harris 220. +c +c data mcheps(1),mcheps(2) / '20000000, '00000334 / +c data minmag(1),minmag(2) / '20000000, '00000201 / +c data maxmag(1),maxmag(2) / '37777777, '37777577 / +c +c Machine constants for the Cray-1. +c +c data mcheps(1) / 0376424000000000000000b / +c data mcheps(2) / 0000000000000000000000b / +c +c data minmag(1) / 0200034000000000000000b / +c data minmag(2) / 0000000000000000000000b / +c +c data maxmag(1) / 0577777777777777777777b / +c data maxmag(2) / 0000007777777777777776b / +c +c Machine constants for the Prime 400. +c +c data mcheps(1),mcheps(2) / :10000000000, :00000000123 / +c data minmag(1),minmag(2) / :10000000000, :00000100000 / +c data maxmag(1),maxmag(2) / :17777777777, :37777677776 / +c +c Machine constants for the VAX-11. +c +c data mcheps(1),mcheps(2) / 9472, 0 / +c data minmag(1),minmag(2) / 128, 0 / +c data maxmag(1),maxmag(2) / -32769, -1 / +c +c Machine constants for the IEEE double precision floating point. +c taken from values of dmach(4), dmach(1) and dmach(2) in d1mach.f, +c generated by machar.f from the BLAS distribution in Netlib. +c + data mcheps(1),mcheps(2) / 1018167296 , 0 / + data minmag(1),minmag(2) / 1048576 , 0 / + data maxmag(1),maxmag(2) / 2146435071 , -1 / +c + dpmpar = dmach(i) + return +c +c Last card of function dpmpar. +c + end diff --git a/others/pyTsai/src/minpack/enorm.c b/others/pyTsai/src/minpack/enorm.c new file mode 100644 index 0000000..c57eab1 --- /dev/null +++ b/others/pyTsai/src/minpack/enorm.c @@ -0,0 +1,166 @@ +/* enorm.f -- translated by f2c (version of 17 January 1992 0:17:58). + You must link the resulting object file with the libraries: + -lf77 -li77 -lm -lc (in that order) +*/ + +#include "f2c.h" + +doublereal enorm_(n, x) +integer *n; +doublereal *x; +{ + /* Initialized data */ + + static doublereal one = 1.; + static doublereal zero = 0.; + static doublereal rdwarf = 3.834e-20; + static doublereal rgiant = 1.304e19; + + /* System generated locals */ + integer i__1; + doublereal ret_val = 0., d__1; + + /* Builtin functions */ + double sqrt(); + + /* Local variables */ + static doublereal xabs, x1max, x3max; + static integer i; + static doublereal s1, s2, s3, agiant, floatn; + +/* ********** */ + +/* function enorm */ + +/* given an n-vector x, this function calculates the */ +/* euclidean norm of x. */ + +/* the euclidean norm is computed by accumulating the sum of */ +/* squares in three different sums. the sums of squares for the */ +/* small and large components are scaled so that no overflows */ +/* occur. non-destructive underflows are permitted. underflows */ +/* and overflows do not occur in the computation of the unscaled */ +/* sum of squares for the intermediate components. */ +/* the definitions of small, intermediate and large components */ +/* depend on two constants, rdwarf and rgiant. the main */ +/* restrictions on these constants are that rdwarf**2 not */ +/* underflow and rgiant**2 not overflow. the constants */ +/* given here are suitable for every known computer. */ + +/* the function statement is */ + +/* double precision function enorm(n,x) */ + +/* where */ + +/* n is a positive integer input variable. */ + +/* x is an input array of length n. */ + +/* subprograms called */ + +/* fortran-supplied ... dabs,dsqrt */ + +/* argonne national laboratory. minpack project. march 1980. */ +/* burton s. garbow, kenneth e. hillstrom, jorge j. more */ + +/* ********** */ + /* Parameter adjustments */ + --x; + + /* Function Body */ + s1 = zero; + s2 = zero; + s3 = zero; + x1max = zero; + x3max = zero; + floatn = (doublereal) (*n); + agiant = rgiant / floatn; + i__1 = *n; + for (i = 1; i <= i__1; ++i) { + xabs = (d__1 = x[i], abs(d__1)); + if (xabs > rdwarf && xabs < agiant) { + goto L70; + } + if (xabs <= rdwarf) { + goto L30; + } + +/* sum for large components. */ + + if (xabs <= x1max) { + goto L10; + } +/* Computing 2nd power */ + d__1 = x1max / xabs; + s1 = one + s1 * (d__1 * d__1); + x1max = xabs; + goto L20; +L10: +/* Computing 2nd power */ + d__1 = xabs / x1max; + s1 += d__1 * d__1; +L20: + goto L60; +L30: + +/* sum for small components. */ + + if (xabs <= x3max) { + goto L40; + } +/* Computing 2nd power */ + d__1 = x3max / xabs; + s3 = one + s3 * (d__1 * d__1); + x3max = xabs; + goto L50; +L40: + if (xabs != zero) { +/* Computing 2nd power */ + d__1 = xabs / x3max; + s3 += d__1 * d__1; + } +L50: +L60: + goto L80; +L70: + +/* sum for intermediate components. */ + +/* Computing 2nd power */ + d__1 = xabs; + s2 += d__1 * d__1; +L80: +/* L90: */ + ; + } + +/* calculation of norm. */ + + if (s1 == zero) { + goto L100; + } + ret_val = x1max * sqrt(s1 + s2 / x1max / x1max); + goto L130; +L100: + if (s2 == zero) { + goto L110; + } + if (s2 >= x3max) { + d__1 = x3max * s3; + ret_val = sqrt(s2 * (one + x3max / s2 * d__1)); + } + if (s2 < x3max) { + ret_val = sqrt(x3max * (s2 / x3max + x3max * s3)); + } + goto L120; +L110: + ret_val = x3max * sqrt(s3); +L120: +L130: + return ret_val; + +/* last card of function enorm. */ + +} /* enorm_ */ + diff --git a/others/pyTsai/src/minpack/enorm.f b/others/pyTsai/src/minpack/enorm.f new file mode 100644 index 0000000..2cb5b60 --- /dev/null +++ b/others/pyTsai/src/minpack/enorm.f @@ -0,0 +1,108 @@ + double precision function enorm(n,x) + integer n + double precision x(n) +c ********** +c +c function enorm +c +c given an n-vector x, this function calculates the +c euclidean norm of x. +c +c the euclidean norm is computed by accumulating the sum of +c squares in three different sums. the sums of squares for the +c small and large components are scaled so that no overflows +c occur. non-destructive underflows are permitted. underflows +c and overflows do not occur in the computation of the unscaled +c sum of squares for the intermediate components. +c the definitions of small, intermediate and large components +c depend on two constants, rdwarf and rgiant. the main +c restrictions on these constants are that rdwarf**2 not +c underflow and rgiant**2 not overflow. the constants +c given here are suitable for every known computer. +c +c the function statement is +c +c double precision function enorm(n,x) +c +c where +c +c n is a positive integer input variable. +c +c x is an input array of length n. +c +c subprograms called +c +c fortran-supplied ... dabs,dsqrt +c +c argonne national laboratory. minpack project. march 1980. +c burton s. garbow, kenneth e. hillstrom, jorge j. more +c +c ********** + integer i + double precision agiant,floatn,one,rdwarf,rgiant,s1,s2,s3,xabs, + * x1max,x3max,zero + data one,zero,rdwarf,rgiant /1.0d0,0.0d0,3.834d-20,1.304d19/ + s1 = zero + s2 = zero + s3 = zero + x1max = zero + x3max = zero + floatn = n + agiant = rgiant/floatn + do 90 i = 1, n + xabs = dabs(x(i)) + if (xabs .gt. rdwarf .and. xabs .lt. agiant) go to 70 + if (xabs .le. rdwarf) go to 30 +c +c sum for large components. +c + if (xabs .le. x1max) go to 10 + s1 = one + s1*(x1max/xabs)**2 + x1max = xabs + go to 20 + 10 continue + s1 = s1 + (xabs/x1max)**2 + 20 continue + go to 60 + 30 continue +c +c sum for small components. +c + if (xabs .le. x3max) go to 40 + s3 = one + s3*(x3max/xabs)**2 + x3max = xabs + go to 50 + 40 continue + if (xabs .ne. zero) s3 = s3 + (xabs/x3max)**2 + 50 continue + 60 continue + go to 80 + 70 continue +c +c sum for intermediate components. +c + s2 = s2 + xabs**2 + 80 continue + 90 continue +c +c calculation of norm. +c + if (s1 .eq. zero) go to 100 + enorm = x1max*dsqrt(s1+(s2/x1max)/x1max) + go to 130 + 100 continue + if (s2 .eq. zero) go to 110 + if (s2 .ge. x3max) + * enorm = dsqrt(s2*(one+(x3max/s2)*(x3max*s3))) + if (s2 .lt. x3max) + * enorm = dsqrt(x3max*((s2/x3max)+(x3max*s3))) + go to 120 + 110 continue + enorm = x3max*dsqrt(s3) + 120 continue + 130 continue + return +c +c last card of function enorm. +c + end diff --git a/others/pyTsai/src/minpack/f2c.h b/others/pyTsai/src/minpack/f2c.h new file mode 100644 index 0000000..1979397 --- /dev/null +++ b/others/pyTsai/src/minpack/f2c.h @@ -0,0 +1,215 @@ +/* f2c.h -- Standard Fortran to C header file + + f2c is a Fortran to C converter under development by + David Gay (AT&T Bell Labs) + Stu Feldman (Bellcore) + Mark Maimone (Carnegie-Mellon University) + Norm Schryer (AT&T Bell Labs) + +*** barf [ba:rf] 2. "He suggested using FORTRAN, and everybody barfed." + + - From The Shogakukan DICTIONARY OF NEW ENGLISH (Second edition) */ + +#ifndef F2C_INCLUDE +#define F2C_INCLUDE + +typedef long int integer; +typedef char *address; +typedef short int shortint; +typedef float real; +typedef double doublereal; +typedef struct { real r, i; } complex; +typedef struct { doublereal r, i; } doublecomplex; +typedef long int logical; +typedef short int shortlogical; + +#define TRUE_ (1) +#define FALSE_ (0) + +/* Extern is for use with -E */ +#ifndef Extern +#define Extern extern +#endif + +/* I/O stuff */ + +#ifdef f2c_i2 +/* for -i2 */ +typedef short flag; +typedef short ftnlen; +typedef short ftnint; +#else +typedef long flag; +typedef long ftnlen; +typedef long ftnint; +#endif + +/*external read, write*/ +typedef struct +{ flag cierr; + ftnint ciunit; + flag ciend; + char *cifmt; + ftnint cirec; +} cilist; + +/*internal read, write*/ +typedef struct +{ flag icierr; + char *iciunit; + flag iciend; + char *icifmt; + ftnint icirlen; + ftnint icirnum; +} icilist; + +/*open*/ +typedef struct +{ flag oerr; + ftnint ounit; + char *ofnm; + ftnlen ofnmlen; + char *osta; + char *oacc; + char *ofm; + ftnint orl; + char *oblnk; +} olist; + +/*close*/ +typedef struct +{ flag cerr; + ftnint cunit; + char *csta; +} cllist; + +/*rewind, backspace, endfile*/ +typedef struct +{ flag aerr; + ftnint aunit; +} alist; + +/* inquire */ +typedef struct +{ flag inerr; + ftnint inunit; + char *infile; + ftnlen infilen; + ftnint *inex; /*parameters in standard's order*/ + ftnint *inopen; + ftnint *innum; + ftnint *innamed; + char *inname; + ftnlen innamlen; + char *inacc; + ftnlen inacclen; + char *inseq; + ftnlen inseqlen; + char *indir; + ftnlen indirlen; + char *infmt; + ftnlen infmtlen; + char *inform; + ftnint informlen; + char *inunf; + ftnlen inunflen; + ftnint *inrecl; + ftnint *innrec; + char *inblank; + ftnlen inblanklen; +} inlist; + +#define VOID void + +union Multitype { /* for multiple entry points */ + shortint h; + integer i; + real r; + doublereal d; + complex c; + doublecomplex z; + }; + +typedef union Multitype Multitype; + +typedef long Long; /* No longer used; formerly in Namelist */ + +struct Vardesc { /* for Namelist */ + char *name; + char *addr; + ftnlen *dims; + int type; + }; +typedef struct Vardesc Vardesc; + +struct Namelist { + char *name; + Vardesc **vars; + int nvars; + }; +typedef struct Namelist Namelist; + +#define abs(x) ((x) >= 0 ? (x) : -(x)) +#define dabs(x) (doublereal)abs(x) +#define min(a,b) ((a) <= (b) ? (a) : (b)) +#define max(a,b) ((a) >= (b) ? (a) : (b)) +#define dmin(a,b) (doublereal)min(a,b) +#define dmax(a,b) (doublereal)max(a,b) + +/* procedure parameter types for -A and -C++ */ + +#define F2C_proc_par_types 1 +#ifdef __cplusplus +typedef int /* Unknown procedure type */ (*U_fp)(...); +typedef shortint (*J_fp)(...); +typedef integer (*I_fp)(...); +typedef real (*R_fp)(...); +typedef doublereal (*D_fp)(...), (*E_fp)(...); +typedef /* Complex */ VOID (*C_fp)(...); +typedef /* Double Complex */ VOID (*Z_fp)(...); +typedef logical (*L_fp)(...); +typedef shortlogical (*K_fp)(...); +typedef /* Character */ VOID (*H_fp)(...); +typedef /* Subroutine */ int (*S_fp)(...); +#else +typedef int /* Unknown procedure type */ (*U_fp)(); +typedef shortint (*J_fp)(); +typedef integer (*I_fp)(); +typedef real (*R_fp)(); +typedef doublereal (*D_fp)(), (*E_fp)(); +typedef /* Complex */ VOID (*C_fp)(); +typedef /* Double Complex */ VOID (*Z_fp)(); +typedef logical (*L_fp)(); +typedef shortlogical (*K_fp)(); +typedef /* Character */ VOID (*H_fp)(); +typedef /* Subroutine */ int (*S_fp)(); +#endif +/* E_fp is for real functions when -R is not specified */ +#define C_f VOID /* complex function */ +#define H_f VOID /* character function */ +#define Z_f VOID /* double complex function */ +typedef doublereal E_f; /* real function with -R not specified */ + +/* undef any lower-case symbols that your C compiler predefines, e.g.: */ + +#ifndef Skip_f2c_Undefs +#undef cray +#undef gcos +#undef mc68010 +#undef mc68020 +#undef mips +#undef pdp11 +#undef sgi +#undef sparc +#undef sun +#undef sun2 +#undef sun3 +#undef sun4 +#undef u370 +#undef u3b +#undef u3b2 +#undef u3b5 +#undef unix +#undef vax +#endif +#endif diff --git a/others/pyTsai/src/minpack/f2c.ps b/others/pyTsai/src/minpack/f2c.ps new file mode 100644 index 0000000..8450aa0 Binary files /dev/null and b/others/pyTsai/src/minpack/f2c.ps differ diff --git a/others/pyTsai/src/minpack/fdjac2.c b/others/pyTsai/src/minpack/fdjac2.c new file mode 100644 index 0000000..d2ede30 --- /dev/null +++ b/others/pyTsai/src/minpack/fdjac2.c @@ -0,0 +1,153 @@ +/* fdjac2.f -- translated by f2c (version of 17 January 1992 0:17:58). + You must link the resulting object file with the libraries: + -lf77 -li77 -lm -lc (in that order) +*/ + +#include "f2c.h" + +/* Table of constant values */ + +static integer c__1 = 1; + +/* Subroutine */ int fdjac2_(fcn, m, n, x, fvec, fjac, ldfjac, iflag, epsfcn, + wa) +/* Subroutine */ int (*fcn) (); +integer *m, *n; +doublereal *x, *fvec, *fjac; +integer *ldfjac, *iflag; +doublereal *epsfcn, *wa; +{ + /* Initialized data */ + + static doublereal zero = 0.; + + /* System generated locals */ + integer fjac_dim1, fjac_offset, i__1, i__2; + + /* Builtin functions */ + double sqrt(); + + /* Local variables */ + static doublereal temp, h; + static integer i, j; + static doublereal epsmch; + extern doublereal dpmpar_(); + static doublereal eps; + +/* ********** */ + +/* subroutine fdjac2 */ + +/* this subroutine computes a forward-difference approximation */ +/* to the m by n jacobian matrix associated with a specified */ +/* problem of m functions in n variables. */ + +/* the subroutine statement is */ + +/* subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa) */ + +/* where */ + +/* fcn is the name of the user-supplied subroutine which */ +/* calculates the functions. fcn must be declared */ +/* in an external statement in the user calling */ +/* program, and should be written as follows. */ + +/* subroutine fcn(m,n,x,fvec,iflag) */ +/* integer m,n,iflag */ +/* double precision x(n),fvec(m) */ +/* ---------- */ +/* calculate the functions at x and */ +/* return this vector in fvec. */ +/* ---------- */ +/* return */ +/* end */ + +/* the value of iflag should not be changed by fcn unless */ +/* the user wants to terminate execution of fdjac2. */ +/* in this case set iflag to a negative integer. */ + +/* m is a positive integer input variable set to the number */ +/* of functions. */ + +/* n is a positive integer input variable set to the number */ +/* of variables. n must not exceed m. */ + +/* x is an input array of length n. */ + +/* fvec is an input array of length m which must contain the */ +/* functions evaluated at x. */ + +/* fjac is an output m by n array which contains the */ +/* approximation to the jacobian matrix evaluated at x. */ + +/* ldfjac is a positive integer input variable not less than m */ +/* which specifies the leading dimension of the array fjac. */ + +/* iflag is an integer variable which can be used to terminate */ +/* the execution of fdjac2. see description of fcn. */ + +/* epsfcn is an input variable used in determining a suitable */ +/* step length for the forward-difference approximation. this */ +/* approximation assumes that the relative errors in the */ +/* functions are of the order of epsfcn. if epsfcn is less */ +/* than the machine precision, it is assumed that the relative */ +/* errors in the functions are of the order of the machine */ +/* precision. */ + +/* wa is a work array of length m. */ + +/* subprograms called */ + +/* user-supplied ...... fcn */ + +/* minpack-supplied ... dpmpar */ + +/* fortran-supplied ... dabs,dmax1,dsqrt */ + +/* argonne national laboratory. minpack project. march 1980. */ +/* burton s. garbow, kenneth e. hillstrom, jorge j. more */ + +/* ********** */ + /* Parameter adjustments */ + --wa; + fjac_dim1 = *ldfjac; + fjac_offset = fjac_dim1 + 1; + fjac -= fjac_offset; + --fvec; + --x; + + /* Function Body */ + +/* epsmch is the machine precision. */ + + epsmch = dpmpar_(&c__1); + + eps = sqrt((max(*epsfcn,epsmch))); + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + temp = x[j]; + h = eps * abs(temp); + if (h == zero) { + h = eps; + } + x[j] = temp + h; + (*fcn)(m, n, &x[1], &wa[1], iflag); + if (*iflag < 0) { + goto L30; + } + x[j] = temp; + i__2 = *m; + for (i = 1; i <= i__2; ++i) { + fjac[i + j * fjac_dim1] = (wa[i] - fvec[i]) / h; +/* L10: */ + } +/* L20: */ + } +L30: + return 0; + +/* last card of subroutine fdjac2. */ + +} /* fdjac2_ */ + diff --git a/others/pyTsai/src/minpack/fdjac2.f b/others/pyTsai/src/minpack/fdjac2.f new file mode 100644 index 0000000..218ab94 --- /dev/null +++ b/others/pyTsai/src/minpack/fdjac2.f @@ -0,0 +1,107 @@ + subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa) + integer m,n,ldfjac,iflag + double precision epsfcn + double precision x(n),fvec(m),fjac(ldfjac,n),wa(m) +c ********** +c +c subroutine fdjac2 +c +c this subroutine computes a forward-difference approximation +c to the m by n jacobian matrix associated with a specified +c problem of m functions in n variables. +c +c the subroutine statement is +c +c subroutine fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa) +c +c where +c +c fcn is the name of the user-supplied subroutine which +c calculates the functions. fcn must be declared +c in an external statement in the user calling +c program, and should be written as follows. +c +c subroutine fcn(m,n,x,fvec,iflag) +c integer m,n,iflag +c double precision x(n),fvec(m) +c ---------- +c calculate the functions at x and +c return this vector in fvec. +c ---------- +c return +c end +c +c the value of iflag should not be changed by fcn unless +c the user wants to terminate execution of fdjac2. +c in this case set iflag to a negative integer. +c +c m is a positive integer input variable set to the number +c of functions. +c +c n is a positive integer input variable set to the number +c of variables. n must not exceed m. +c +c x is an input array of length n. +c +c fvec is an input array of length m which must contain the +c functions evaluated at x. +c +c fjac is an output m by n array which contains the +c approximation to the jacobian matrix evaluated at x. +c +c ldfjac is a positive integer input variable not less than m +c which specifies the leading dimension of the array fjac. +c +c iflag is an integer variable which can be used to terminate +c the execution of fdjac2. see description of fcn. +c +c epsfcn is an input variable used in determining a suitable +c step length for the forward-difference approximation. this +c approximation assumes that the relative errors in the +c functions are of the order of epsfcn. if epsfcn is less +c than the machine precision, it is assumed that the relative +c errors in the functions are of the order of the machine +c precision. +c +c wa is a work array of length m. +c +c subprograms called +c +c user-supplied ...... fcn +c +c minpack-supplied ... dpmpar +c +c fortran-supplied ... dabs,dmax1,dsqrt +c +c argonne national laboratory. minpack project. march 1980. +c burton s. garbow, kenneth e. hillstrom, jorge j. more +c +c ********** + integer i,j + double precision eps,epsmch,h,temp,zero + double precision dpmpar + data zero /0.0d0/ +c +c epsmch is the machine precision. +c + epsmch = dpmpar(1) +c + eps = dsqrt(dmax1(epsfcn,epsmch)) + do 20 j = 1, n + temp = x(j) + h = eps*dabs(temp) + if (h .eq. zero) h = eps + x(j) = temp + h + call fcn(m,n,x,wa,iflag) + if (iflag .lt. 0) go to 30 + x(j) = temp + do 10 i = 1, m + fjac(i,j) = (wa(i) - fvec(i))/h + 10 continue + 20 continue + 30 continue + return +c +c last card of subroutine fdjac2. +c + end diff --git a/others/pyTsai/src/minpack/lmdif.c b/others/pyTsai/src/minpack/lmdif.c new file mode 100644 index 0000000..637f8b6 --- /dev/null +++ b/others/pyTsai/src/minpack/lmdif.c @@ -0,0 +1,636 @@ +/* lmdif.f -- translated by f2c (version of 17 January 1992 0:17:58). + You must link the resulting object file with the libraries: + -lf77 -li77 -lm -lc (in that order) +*/ + +#include "f2c.h" + +/* Table of constant values */ + +static integer c__1 = 1; + +/* Subroutine */ int lmdif_(fcn, m, n, x, fvec, ftol, xtol, gtol, maxfev, + epsfcn, diag, mode, factor, nprint, info, nfev, fjac, ldfjac, ipvt, + qtf, wa1, wa2, wa3, wa4) +/* Subroutine */ int (*fcn) (); +integer *m, *n; +doublereal *x, *fvec, *ftol, *xtol, *gtol; +integer *maxfev; +doublereal *epsfcn, *diag; +integer *mode; +doublereal *factor; +integer *nprint, *info, *nfev; +doublereal *fjac; +integer *ldfjac, *ipvt; +doublereal *qtf, *wa1, *wa2, *wa3, *wa4; +{ + /* Initialized data */ + + static doublereal one = 1.; + static doublereal p1 = .1; + static doublereal p5 = .5; + static doublereal p25 = .25; + static doublereal p75 = .75; + static doublereal p0001 = 1e-4; + static doublereal zero = 0.; + + /* System generated locals */ + integer fjac_dim1, fjac_offset, i__1, i__2; + doublereal d__1, d__2, d__3; + + /* Builtin functions */ + double sqrt(); + + /* Local variables */ + static integer iter; + static doublereal temp, temp1, temp2; + static integer i, j, l, iflag; + static doublereal delta; + extern /* Subroutine */ int qrfac_(), lmpar_(); + static doublereal ratio; + extern doublereal enorm_(); + static doublereal fnorm, gnorm; + extern /* Subroutine */ int fdjac2_(); + static doublereal pnorm, xnorm, fnorm1, actred, dirder, epsmch, prered; + extern doublereal dpmpar_(); + static doublereal par, sum; + +/* ********** */ + +/* subroutine lmdif */ + +/* the purpose of lmdif is to minimize the sum of the squares of */ +/* m nonlinear functions in n variables by a modification of */ +/* the levenberg-marquardt algorithm. the user must provide a */ +/* subroutine which calculates the functions. the jacobian is */ +/* then calculated by a forward-difference approximation. */ + +/* the subroutine statement is */ + +/* subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn, */ +/* diag,mode,factor,nprint,info,nfev,fjac, */ +/* ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4) */ + +/* where */ + +/* fcn is the name of the user-supplied subroutine which */ +/* calculates the functions. fcn must be declared */ +/* in an external statement in the user calling */ +/* program, and should be written as follows. */ + +/* subroutine fcn(m,n,x,fvec,iflag) */ +/* integer m,n,iflag */ +/* double precision x(n),fvec(m) */ +/* ---------- */ +/* calculate the functions at x and */ +/* return this vector in fvec. */ +/* ---------- */ +/* return */ +/* end */ + +/* the value of iflag should not be changed by fcn unless */ +/* the user wants to terminate execution of lmdif. */ +/* in this case set iflag to a negative integer. */ + +/* m is a positive integer input variable set to the number */ +/* of functions. */ + +/* n is a positive integer input variable set to the number */ +/* of variables. n must not exceed m. */ + +/* x is an array of length n. on input x must contain */ +/* an initial estimate of the solution vector. on output x */ +/* contains the final estimate of the solution vector. */ + +/* fvec is an output array of length m which contains */ +/* the functions evaluated at the output x. */ + +/* ftol is a nonnegative input variable. termination */ +/* occurs when both the actual and predicted relative */ +/* reductions in the sum of squares are at most ftol. */ +/* therefore, ftol measures the relative error desired */ +/* in the sum of squares. */ + +/* xtol is a nonnegative input variable. termination */ +/* occurs when the relative error between two consecutive */ +/* iterates is at most xtol. therefore, xtol measures the */ +/* relative error desired in the approximate solution. */ + +/* gtol is a nonnegative input variable. termination */ +/* occurs when the cosine of the angle between fvec and */ +/* any column of the jacobian is at most gtol in absolute */ +/* value. therefore, gtol measures the orthogonality */ +/* desired between the function vector and the columns */ +/* of the jacobian. */ + +/* maxfev is a positive integer input variable. termination */ +/* occurs when the number of calls to fcn is at least */ +/* maxfev by the end of an iteration. */ + +/* epsfcn is an input variable used in determining a suitable */ +/* step length for the forward-difference approximation. this */ +/* approximation assumes that the relative errors in the */ +/* functions are of the order of epsfcn. if epsfcn is less */ +/* than the machine precision, it is assumed that the relative */ +/* errors in the functions are of the order of the machine */ +/* precision. */ + +/* diag is an array of length n. if mode = 1 (see */ +/* below), diag is internally set. if mode = 2, diag */ +/* must contain positive entries that serve as */ +/* multiplicative scale factors for the variables. */ + +/* mode is an integer input variable. if mode = 1, the */ +/* variables will be scaled internally. if mode = 2, */ +/* the scaling is specified by the input diag. other */ +/* values of mode are equivalent to mode = 1. */ + +/* factor is a positive input variable used in determining the */ +/* initial step bound. this bound is set to the product of */ +/* factor and the euclidean norm of diag*x if nonzero, or else */ +/* to factor itself. in most cases factor should lie in the */ +/* interval (.1,100.). 100. is a generally recommended value. */ + +/* nprint is an integer input variable that enables controlled */ +/* printing of iterates if it is positive. in this case, */ +/* fcn is called with iflag = 0 at the beginning of the first */ +/* iteration and every nprint iterations thereafter and */ +/* immediately prior to return, with x and fvec available */ +/* for printing. if nprint is not positive, no special calls */ +/* of fcn with iflag = 0 are made. */ + +/* info is an integer output variable. if the user has */ +/* terminated execution, info is set to the (negative) */ +/* value of iflag. see description of fcn. otherwise, */ +/* info is set as follows. */ + +/* info = 0 improper input parameters. */ + +/* info = 1 both actual and predicted relative reductions */ +/* in the sum of squares are at most ftol. */ + +/* info = 2 relative error between two consecutive iterates */ +/* is at most xtol. */ + +/* info = 3 conditions for info = 1 and info = 2 both hold. */ + +/* info = 4 the cosine of the angle between fvec and any */ +/* column of the jacobian is at most gtol in */ +/* absolute value. */ + +/* info = 5 number of calls to fcn has reached or */ +/* exceeded maxfev. */ + +/* info = 6 ftol is too small. no further reduction in */ +/* the sum of squares is possible. */ + +/* info = 7 xtol is too small. no further improvement in */ +/* the approximate solution x is possible. */ + +/* info = 8 gtol is too small. fvec is orthogonal to the */ +/* columns of the jacobian to machine precision. */ + +/* nfev is an integer output variable set to the number of */ +/* calls to fcn. */ + +/* fjac is an output m by n array. the upper n by n submatrix */ +/* of fjac contains an upper triangular matrix r with */ +/* diagonal elements of nonincreasing magnitude such that */ + +/* t t t */ +/* p *(jac *jac)*p = r *r, */ + +/* where p is a permutation matrix and jac is the final */ +/* calculated jacobian. column j of p is column ipvt(j) */ +/* (see below) of the identity matrix. the lower trapezoidal */ +/* part of fjac contains information generated during */ +/* the computation of r. */ + +/* ldfjac is a positive integer input variable not less than m */ +/* which specifies the leading dimension of the array fjac. */ + +/* ipvt is an integer output array of length n. ipvt */ +/* defines a permutation matrix p such that jac*p = q*r, */ +/* where jac is the final calculated jacobian, q is */ +/* orthogonal (not stored), and r is upper triangular */ +/* with diagonal elements of nonincreasing magnitude. */ +/* column j of p is column ipvt(j) of the identity matrix. */ + +/* qtf is an output array of length n which contains */ +/* the first n elements of the vector (q transpose)*fvec. */ + +/* wa1, wa2, and wa3 are work arrays of length n. */ + +/* wa4 is a work array of length m. */ + +/* subprograms called */ + +/* user-supplied ...... fcn */ + +/* minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac */ + +/* fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod */ + +/* argonne national laboratory. minpack project. march 1980. */ +/* burton s. garbow, kenneth e. hillstrom, jorge j. more */ + +/* ********** */ + /* Parameter adjustments */ + --wa4; + --wa3; + --wa2; + --wa1; + --qtf; + --ipvt; + fjac_dim1 = *ldfjac; + fjac_offset = fjac_dim1 + 1; + fjac -= fjac_offset; + --diag; + --fvec; + --x; + + /* Function Body */ + +/* epsmch is the machine precision. */ + + epsmch = dpmpar_(&c__1); + + *info = 0; + iflag = 0; + *nfev = 0; + +/* check the input parameters for errors. */ + + if (*n <= 0 || *m < *n || *ldfjac < *m || *ftol < zero || *xtol < zero || + *gtol < zero || *maxfev <= 0 || *factor <= zero) { + goto L300; + } + if (*mode != 2) { + goto L20; + } + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + if (diag[j] <= zero) { + goto L300; + } +/* L10: */ + } +L20: + +/* evaluate the function at the starting point */ +/* and calculate its norm. */ + + iflag = 1; + (*fcn)(m, n, &x[1], &fvec[1], &iflag); + *nfev = 1; + if (iflag < 0) { + goto L300; + } + fnorm = enorm_(m, &fvec[1]); + +/* initialize levenberg-marquardt parameter and iteration counter. */ + + par = zero; + iter = 1; + +/* beginning of the outer loop. */ + +L30: + +/* calculate the jacobian matrix. */ + + iflag = 2; + fdjac2_(fcn, m, n, &x[1], &fvec[1], &fjac[fjac_offset], ldfjac, &iflag, + epsfcn, &wa4[1]); + *nfev += *n; + if (iflag < 0) { + goto L300; + } + +/* if requested, call fcn to enable printing of iterates. */ + + if (*nprint <= 0) { + goto L40; + } + iflag = 0; + if ((iter - 1) % *nprint == 0) { + (*fcn)(m, n, &x[1], &fvec[1], &iflag); + } + if (iflag < 0) { + goto L300; + } +L40: + +/* compute the qr factorization of the jacobian. */ + + qrfac_(m, n, &fjac[fjac_offset], ldfjac, &c__1, &ipvt[1], n, &wa1[1], & + wa2[1], &wa3[1]); + +/* on the first iteration and if mode is 1, scale according */ +/* to the norms of the columns of the initial jacobian. */ + + if (iter != 1) { + goto L80; + } + if (*mode == 2) { + goto L60; + } + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + diag[j] = wa2[j]; + if (wa2[j] == zero) { + diag[j] = one; + } +/* L50: */ + } +L60: + +/* on the first iteration, calculate the norm of the scaled x */ +/* and initialize the step bound delta. */ + + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + wa3[j] = diag[j] * x[j]; +/* L70: */ + } + xnorm = enorm_(n, &wa3[1]); + delta = *factor * xnorm; + if (delta == zero) { + delta = *factor; + } +L80: + +/* form (q transpose)*fvec and store the first n components in */ +/* qtf. */ + + i__1 = *m; + for (i = 1; i <= i__1; ++i) { + wa4[i] = fvec[i]; +/* L90: */ + } + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + if (fjac[j + j * fjac_dim1] == zero) { + goto L120; + } + sum = zero; + i__2 = *m; + for (i = j; i <= i__2; ++i) { + sum += fjac[i + j * fjac_dim1] * wa4[i]; +/* L100: */ + } + temp = -sum / fjac[j + j * fjac_dim1]; + i__2 = *m; + for (i = j; i <= i__2; ++i) { + wa4[i] += fjac[i + j * fjac_dim1] * temp; +/* L110: */ + } +L120: + fjac[j + j * fjac_dim1] = wa1[j]; + qtf[j] = wa4[j]; +/* L130: */ + } + +/* compute the norm of the scaled gradient. */ + + gnorm = zero; + if (fnorm == zero) { + goto L170; + } + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + l = ipvt[j]; + if (wa2[l] == zero) { + goto L150; + } + sum = zero; + i__2 = j; + for (i = 1; i <= i__2; ++i) { + sum += fjac[i + j * fjac_dim1] * (qtf[i] / fnorm); +/* L140: */ + } +/* Computing MAX */ + d__2 = gnorm, d__3 = (d__1 = sum / wa2[l], abs(d__1)); + gnorm = max(d__2,d__3); +L150: +/* L160: */ + ; + } +L170: + +/* test for convergence of the gradient norm. */ + + if (gnorm <= *gtol) { + *info = 4; + } + if (*info != 0) { + goto L300; + } + +/* rescale if necessary. */ + + if (*mode == 2) { + goto L190; + } + i__1 = *n; + for (j = 1; j <= i__1; ++j) { +/* Computing MAX */ + d__1 = diag[j], d__2 = wa2[j]; + diag[j] = max(d__1,d__2); +/* L180: */ + } +L190: + +/* beginning of the inner loop. */ + +L200: + +/* determine the levenberg-marquardt parameter. */ + + lmpar_(n, &fjac[fjac_offset], ldfjac, &ipvt[1], &diag[1], &qtf[1], &delta, + &par, &wa1[1], &wa2[1], &wa3[1], &wa4[1]); + +/* store the direction p and x + p. calculate the norm of p. */ + + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + wa1[j] = -wa1[j]; + wa2[j] = x[j] + wa1[j]; + wa3[j] = diag[j] * wa1[j]; +/* L210: */ + } + pnorm = enorm_(n, &wa3[1]); + +/* on the first iteration, adjust the initial step bound. */ + + if (iter == 1) { + delta = min(delta,pnorm); + } + +/* evaluate the function at x + p and calculate its norm. */ + + iflag = 1; + (*fcn)(m, n, &wa2[1], &wa4[1], &iflag); + ++(*nfev); + if (iflag < 0) { + goto L300; + } + fnorm1 = enorm_(m, &wa4[1]); + +/* compute the scaled actual reduction. */ + + actred = -one; + if (p1 * fnorm1 < fnorm) { +/* Computing 2nd power */ + d__1 = fnorm1 / fnorm; + actred = one - d__1 * d__1; + } + +/* compute the scaled predicted reduction and */ +/* the scaled directional derivative. */ + + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + wa3[j] = zero; + l = ipvt[j]; + temp = wa1[l]; + i__2 = j; + for (i = 1; i <= i__2; ++i) { + wa3[i] += fjac[i + j * fjac_dim1] * temp; +/* L220: */ + } +/* L230: */ + } + temp1 = enorm_(n, &wa3[1]) / fnorm; + temp2 = sqrt(par) * pnorm / fnorm; +/* Computing 2nd power */ + d__1 = temp1; +/* Computing 2nd power */ + d__2 = temp2; + prered = d__1 * d__1 + d__2 * d__2 / p5; +/* Computing 2nd power */ + d__1 = temp1; +/* Computing 2nd power */ + d__2 = temp2; + dirder = -(d__1 * d__1 + d__2 * d__2); + +/* compute the ratio of the actual to the predicted */ +/* reduction. */ + + ratio = zero; + if (prered != zero) { + ratio = actred / prered; + } + +/* update the step bound. */ + + if (ratio > p25) { + goto L240; + } + if (actred >= zero) { + temp = p5; + } + if (actred < zero) { + temp = p5 * dirder / (dirder + p5 * actred); + } + if (p1 * fnorm1 >= fnorm || temp < p1) { + temp = p1; + } +/* Computing MIN */ + d__1 = delta, d__2 = pnorm / p1; + delta = temp * min(d__1,d__2); + par /= temp; + goto L260; +L240: + if (par != zero && ratio < p75) { + goto L250; + } + delta = pnorm / p5; + par = p5 * par; +L250: +L260: + +/* test for successful iteration. */ + + if (ratio < p0001) { + goto L290; + } + +/* successful iteration. update x, fvec, and their norms. */ + + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + x[j] = wa2[j]; + wa2[j] = diag[j] * x[j]; +/* L270: */ + } + i__1 = *m; + for (i = 1; i <= i__1; ++i) { + fvec[i] = wa4[i]; +/* L280: */ + } + xnorm = enorm_(n, &wa2[1]); + fnorm = fnorm1; + ++iter; +L290: + +/* tests for convergence. */ + + if (abs(actred) <= *ftol && prered <= *ftol && p5 * ratio <= one) { + *info = 1; + } + if (delta <= *xtol * xnorm) { + *info = 2; + } + if (abs(actred) <= *ftol && prered <= *ftol && p5 * ratio <= one && *info + == 2) { + *info = 3; + } + if (*info != 0) { + goto L300; + } + +/* tests for termination and stringent tolerances. */ + + if (*nfev >= *maxfev) { + *info = 5; + } + if (abs(actred) <= epsmch && prered <= epsmch && p5 * ratio <= one) { + *info = 6; + } + if (delta <= epsmch * xnorm) { + *info = 7; + } + if (gnorm <= epsmch) { + *info = 8; + } + if (*info != 0) { + goto L300; + } + +/* end of the inner loop. repeat if iteration unsuccessful. */ + + if (ratio < p0001) { + goto L200; + } + +/* end of the outer loop. */ + + goto L30; +L300: + +/* termination, either normal or user imposed. */ + + if (iflag < 0) { + *info = iflag; + } + iflag = 0; + if (*nprint > 0) { + (*fcn)(m, n, &x[1], &fvec[1], &iflag); + } + return 0; + +/* last card of subroutine lmdif. */ + +} /* lmdif_ */ + diff --git a/others/pyTsai/src/minpack/lmdif.f b/others/pyTsai/src/minpack/lmdif.f new file mode 100644 index 0000000..dd3d4ee --- /dev/null +++ b/others/pyTsai/src/minpack/lmdif.f @@ -0,0 +1,454 @@ + subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn, + * diag,mode,factor,nprint,info,nfev,fjac,ldfjac, + * ipvt,qtf,wa1,wa2,wa3,wa4) + integer m,n,maxfev,mode,nprint,info,nfev,ldfjac + integer ipvt(n) + double precision ftol,xtol,gtol,epsfcn,factor + double precision x(n),fvec(m),diag(n),fjac(ldfjac,n),qtf(n), + * wa1(n),wa2(n),wa3(n),wa4(m) + external fcn +c ********** +c +c subroutine lmdif +c +c the purpose of lmdif is to minimize the sum of the squares of +c m nonlinear functions in n variables by a modification of +c the levenberg-marquardt algorithm. the user must provide a +c subroutine which calculates the functions. the jacobian is +c then calculated by a forward-difference approximation. +c +c the subroutine statement is +c +c subroutine lmdif(fcn,m,n,x,fvec,ftol,xtol,gtol,maxfev,epsfcn, +c diag,mode,factor,nprint,info,nfev,fjac, +c ldfjac,ipvt,qtf,wa1,wa2,wa3,wa4) +c +c where +c +c fcn is the name of the user-supplied subroutine which +c calculates the functions. fcn must be declared +c in an external statement in the user calling +c program, and should be written as follows. +c +c subroutine fcn(m,n,x,fvec,iflag) +c integer m,n,iflag +c double precision x(n),fvec(m) +c ---------- +c calculate the functions at x and +c return this vector in fvec. +c ---------- +c return +c end +c +c the value of iflag should not be changed by fcn unless +c the user wants to terminate execution of lmdif. +c in this case set iflag to a negative integer. +c +c m is a positive integer input variable set to the number +c of functions. +c +c n is a positive integer input variable set to the number +c of variables. n must not exceed m. +c +c x is an array of length n. on input x must contain +c an initial estimate of the solution vector. on output x +c contains the final estimate of the solution vector. +c +c fvec is an output array of length m which contains +c the functions evaluated at the output x. +c +c ftol is a nonnegative input variable. termination +c occurs when both the actual and predicted relative +c reductions in the sum of squares are at most ftol. +c therefore, ftol measures the relative error desired +c in the sum of squares. +c +c xtol is a nonnegative input variable. termination +c occurs when the relative error between two consecutive +c iterates is at most xtol. therefore, xtol measures the +c relative error desired in the approximate solution. +c +c gtol is a nonnegative input variable. termination +c occurs when the cosine of the angle between fvec and +c any column of the jacobian is at most gtol in absolute +c value. therefore, gtol measures the orthogonality +c desired between the function vector and the columns +c of the jacobian. +c +c maxfev is a positive integer input variable. termination +c occurs when the number of calls to fcn is at least +c maxfev by the end of an iteration. +c +c epsfcn is an input variable used in determining a suitable +c step length for the forward-difference approximation. this +c approximation assumes that the relative errors in the +c functions are of the order of epsfcn. if epsfcn is less +c than the machine precision, it is assumed that the relative +c errors in the functions are of the order of the machine +c precision. +c +c diag is an array of length n. if mode = 1 (see +c below), diag is internally set. if mode = 2, diag +c must contain positive entries that serve as +c multiplicative scale factors for the variables. +c +c mode is an integer input variable. if mode = 1, the +c variables will be scaled internally. if mode = 2, +c the scaling is specified by the input diag. other +c values of mode are equivalent to mode = 1. +c +c factor is a positive input variable used in determining the +c initial step bound. this bound is set to the product of +c factor and the euclidean norm of diag*x if nonzero, or else +c to factor itself. in most cases factor should lie in the +c interval (.1,100.). 100. is a generally recommended value. +c +c nprint is an integer input variable that enables controlled +c printing of iterates if it is positive. in this case, +c fcn is called with iflag = 0 at the beginning of the first +c iteration and every nprint iterations thereafter and +c immediately prior to return, with x and fvec available +c for printing. if nprint is not positive, no special calls +c of fcn with iflag = 0 are made. +c +c info is an integer output variable. if the user has +c terminated execution, info is set to the (negative) +c value of iflag. see description of fcn. otherwise, +c info is set as follows. +c +c info = 0 improper input parameters. +c +c info = 1 both actual and predicted relative reductions +c in the sum of squares are at most ftol. +c +c info = 2 relative error between two consecutive iterates +c is at most xtol. +c +c info = 3 conditions for info = 1 and info = 2 both hold. +c +c info = 4 the cosine of the angle between fvec and any +c column of the jacobian is at most gtol in +c absolute value. +c +c info = 5 number of calls to fcn has reached or +c exceeded maxfev. +c +c info = 6 ftol is too small. no further reduction in +c the sum of squares is possible. +c +c info = 7 xtol is too small. no further improvement in +c the approximate solution x is possible. +c +c info = 8 gtol is too small. fvec is orthogonal to the +c columns of the jacobian to machine precision. +c +c nfev is an integer output variable set to the number of +c calls to fcn. +c +c fjac is an output m by n array. the upper n by n submatrix +c of fjac contains an upper triangular matrix r with +c diagonal elements of nonincreasing magnitude such that +c +c t t t +c p *(jac *jac)*p = r *r, +c +c where p is a permutation matrix and jac is the final +c calculated jacobian. column j of p is column ipvt(j) +c (see below) of the identity matrix. the lower trapezoidal +c part of fjac contains information generated during +c the computation of r. +c +c ldfjac is a positive integer input variable not less than m +c which specifies the leading dimension of the array fjac. +c +c ipvt is an integer output array of length n. ipvt +c defines a permutation matrix p such that jac*p = q*r, +c where jac is the final calculated jacobian, q is +c orthogonal (not stored), and r is upper triangular +c with diagonal elements of nonincreasing magnitude. +c column j of p is column ipvt(j) of the identity matrix. +c +c qtf is an output array of length n which contains +c the first n elements of the vector (q transpose)*fvec. +c +c wa1, wa2, and wa3 are work arrays of length n. +c +c wa4 is a work array of length m. +c +c subprograms called +c +c user-supplied ...... fcn +c +c minpack-supplied ... dpmpar,enorm,fdjac2,lmpar,qrfac +c +c fortran-supplied ... dabs,dmax1,dmin1,dsqrt,mod +c +c argonne national laboratory. minpack project. march 1980. +c burton s. garbow, kenneth e. hillstrom, jorge j. more +c +c ********** + integer i,iflag,iter,j,l + double precision actred,delta,dirder,epsmch,fnorm,fnorm1,gnorm, + * one,par,pnorm,prered,p1,p5,p25,p75,p0001,ratio, + * sum,temp,temp1,temp2,xnorm,zero + double precision dpmpar,enorm + data one,p1,p5,p25,p75,p0001,zero + * /1.0d0,1.0d-1,5.0d-1,2.5d-1,7.5d-1,1.0d-4,0.0d0/ +c +c epsmch is the machine precision. +c + epsmch = dpmpar(1) +c + info = 0 + iflag = 0 + nfev = 0 +c +c check the input parameters for errors. +c + if (n .le. 0 .or. m .lt. n .or. ldfjac .lt. m + * .or. ftol .lt. zero .or. xtol .lt. zero .or. gtol .lt. zero + * .or. maxfev .le. 0 .or. factor .le. zero) go to 300 + if (mode .ne. 2) go to 20 + do 10 j = 1, n + if (diag(j) .le. zero) go to 300 + 10 continue + 20 continue +c +c evaluate the function at the starting point +c and calculate its norm. +c + iflag = 1 + call fcn(m,n,x,fvec,iflag) + nfev = 1 + if (iflag .lt. 0) go to 300 + fnorm = enorm(m,fvec) +c +c initialize levenberg-marquardt parameter and iteration counter. +c + par = zero + iter = 1 +c +c beginning of the outer loop. +c + 30 continue +c +c calculate the jacobian matrix. +c + iflag = 2 + call fdjac2(fcn,m,n,x,fvec,fjac,ldfjac,iflag,epsfcn,wa4) + nfev = nfev + n + if (iflag .lt. 0) go to 300 +c +c if requested, call fcn to enable printing of iterates. +c + if (nprint .le. 0) go to 40 + iflag = 0 + if (mod(iter-1,nprint) .eq. 0) call fcn(m,n,x,fvec,iflag) + if (iflag .lt. 0) go to 300 + 40 continue +c +c compute the qr factorization of the jacobian. +c + call qrfac(m,n,fjac,ldfjac,.true.,ipvt,n,wa1,wa2,wa3) +c +c on the first iteration and if mode is 1, scale according +c to the norms of the columns of the initial jacobian. +c + if (iter .ne. 1) go to 80 + if (mode .eq. 2) go to 60 + do 50 j = 1, n + diag(j) = wa2(j) + if (wa2(j) .eq. zero) diag(j) = one + 50 continue + 60 continue +c +c on the first iteration, calculate the norm of the scaled x +c and initialize the step bound delta. +c + do 70 j = 1, n + wa3(j) = diag(j)*x(j) + 70 continue + xnorm = enorm(n,wa3) + delta = factor*xnorm + if (delta .eq. zero) delta = factor + 80 continue +c +c form (q transpose)*fvec and store the first n components in +c qtf. +c + do 90 i = 1, m + wa4(i) = fvec(i) + 90 continue + do 130 j = 1, n + if (fjac(j,j) .eq. zero) go to 120 + sum = zero + do 100 i = j, m + sum = sum + fjac(i,j)*wa4(i) + 100 continue + temp = -sum/fjac(j,j) + do 110 i = j, m + wa4(i) = wa4(i) + fjac(i,j)*temp + 110 continue + 120 continue + fjac(j,j) = wa1(j) + qtf(j) = wa4(j) + 130 continue +c +c compute the norm of the scaled gradient. +c + gnorm = zero + if (fnorm .eq. zero) go to 170 + do 160 j = 1, n + l = ipvt(j) + if (wa2(l) .eq. zero) go to 150 + sum = zero + do 140 i = 1, j + sum = sum + fjac(i,j)*(qtf(i)/fnorm) + 140 continue + gnorm = dmax1(gnorm,dabs(sum/wa2(l))) + 150 continue + 160 continue + 170 continue +c +c test for convergence of the gradient norm. +c + if (gnorm .le. gtol) info = 4 + if (info .ne. 0) go to 300 +c +c rescale if necessary. +c + if (mode .eq. 2) go to 190 + do 180 j = 1, n + diag(j) = dmax1(diag(j),wa2(j)) + 180 continue + 190 continue +c +c beginning of the inner loop. +c + 200 continue +c +c determine the levenberg-marquardt parameter. +c + call lmpar(n,fjac,ldfjac,ipvt,diag,qtf,delta,par,wa1,wa2, + * wa3,wa4) +c +c store the direction p and x + p. calculate the norm of p. +c + do 210 j = 1, n + wa1(j) = -wa1(j) + wa2(j) = x(j) + wa1(j) + wa3(j) = diag(j)*wa1(j) + 210 continue + pnorm = enorm(n,wa3) +c +c on the first iteration, adjust the initial step bound. +c + if (iter .eq. 1) delta = dmin1(delta,pnorm) +c +c evaluate the function at x + p and calculate its norm. +c + iflag = 1 + call fcn(m,n,wa2,wa4,iflag) + nfev = nfev + 1 + if (iflag .lt. 0) go to 300 + fnorm1 = enorm(m,wa4) +c +c compute the scaled actual reduction. +c + actred = -one + if (p1*fnorm1 .lt. fnorm) actred = one - (fnorm1/fnorm)**2 +c +c compute the scaled predicted reduction and +c the scaled directional derivative. +c + do 230 j = 1, n + wa3(j) = zero + l = ipvt(j) + temp = wa1(l) + do 220 i = 1, j + wa3(i) = wa3(i) + fjac(i,j)*temp + 220 continue + 230 continue + temp1 = enorm(n,wa3)/fnorm + temp2 = (dsqrt(par)*pnorm)/fnorm + prered = temp1**2 + temp2**2/p5 + dirder = -(temp1**2 + temp2**2) +c +c compute the ratio of the actual to the predicted +c reduction. +c + ratio = zero + if (prered .ne. zero) ratio = actred/prered +c +c update the step bound. +c + if (ratio .gt. p25) go to 240 + if (actred .ge. zero) temp = p5 + if (actred .lt. zero) + * temp = p5*dirder/(dirder + p5*actred) + if (p1*fnorm1 .ge. fnorm .or. temp .lt. p1) temp = p1 + delta = temp*dmin1(delta,pnorm/p1) + par = par/temp + go to 260 + 240 continue + if (par .ne. zero .and. ratio .lt. p75) go to 250 + delta = pnorm/p5 + par = p5*par + 250 continue + 260 continue +c +c test for successful iteration. +c + if (ratio .lt. p0001) go to 290 +c +c successful iteration. update x, fvec, and their norms. +c + do 270 j = 1, n + x(j) = wa2(j) + wa2(j) = diag(j)*x(j) + 270 continue + do 280 i = 1, m + fvec(i) = wa4(i) + 280 continue + xnorm = enorm(n,wa2) + fnorm = fnorm1 + iter = iter + 1 + 290 continue +c +c tests for convergence. +c + if (dabs(actred) .le. ftol .and. prered .le. ftol + * .and. p5*ratio .le. one) info = 1 + if (delta .le. xtol*xnorm) info = 2 + if (dabs(actred) .le. ftol .and. prered .le. ftol + * .and. p5*ratio .le. one .and. info .eq. 2) info = 3 + if (info .ne. 0) go to 300 +c +c tests for termination and stringent tolerances. +c + if (nfev .ge. maxfev) info = 5 + if (dabs(actred) .le. epsmch .and. prered .le. epsmch + * .and. p5*ratio .le. one) info = 6 + if (delta .le. epsmch*xnorm) info = 7 + if (gnorm .le. epsmch) info = 8 + if (info .ne. 0) go to 300 +c +c end of the inner loop. repeat if iteration unsuccessful. +c + if (ratio .lt. p0001) go to 200 +c +c end of the outer loop. +c + go to 30 + 300 continue +c +c termination, either normal or user imposed. +c + if (iflag .lt. 0) info = iflag + iflag = 0 + if (nprint .gt. 0) call fcn(m,n,x,fvec,iflag) + return +c +c last card of subroutine lmdif. +c + end diff --git a/others/pyTsai/src/minpack/lmpar.c b/others/pyTsai/src/minpack/lmpar.c new file mode 100644 index 0000000..388d97e --- /dev/null +++ b/others/pyTsai/src/minpack/lmpar.c @@ -0,0 +1,379 @@ +/* lmpar.f -- translated by f2c (version of 17 January 1992 0:17:58). + You must link the resulting object file with the libraries: + -lf77 -li77 -lm -lc (in that order) +*/ + +#include "f2c.h" + +/* Table of constant values */ + +static integer c__2 = 2; + +/* Subroutine */ int lmpar_(n, r, ldr, ipvt, diag, qtb, delta, par, x, sdiag, + wa1, wa2) +integer *n; +doublereal *r; +integer *ldr, *ipvt; +doublereal *diag, *qtb, *delta, *par, *x, *sdiag, *wa1, *wa2; +{ + /* Initialized data */ + + static doublereal p1 = .1; + static doublereal p001 = .001; + static doublereal zero = 0.; + + /* System generated locals */ + integer r_dim1, r_offset, i__1, i__2; + doublereal d__1, d__2; + + /* Builtin functions */ + double sqrt(); + + /* Local variables */ + static doublereal parc, parl; + static integer iter; + static doublereal temp, paru; + static integer i, j, k, l; + static doublereal dwarf; + static integer nsing; + extern doublereal enorm_(); + static doublereal gnorm, fp; + extern doublereal dpmpar_(); + static doublereal dxnorm; + static integer jm1, jp1; + extern /* Subroutine */ int qrsolv_(); + static doublereal sum; + +/* ********** */ + +/* subroutine lmpar */ + +/* given an m by n matrix a, an n by n nonsingular diagonal */ +/* matrix d, an m-vector b, and a positive number delta, */ +/* the problem is to determine a value for the parameter */ +/* par such that if x solves the system */ + +/* a*x = b , sqrt(par)*d*x = 0 , */ + +/* in the least squares sense, and dxnorm is the euclidean */ +/* norm of d*x, then either par is zero and */ + +/* (dxnorm-delta) .le. 0.1*delta , */ + +/* or par is positive and */ + +/* abs(dxnorm-delta) .le. 0.1*delta . */ + +/* this subroutine completes the solution of the problem */ +/* if it is provided with the necessary information from the */ +/* qr factorization, with column pivoting, of a. that is, if */ +/* a*p = q*r, where p is a permutation matrix, q has orthogonal */ +/* columns, and r is an upper triangular matrix with diagonal */ +/* elements of nonincreasing magnitude, then lmpar expects */ +/* the full upper triangle of r, the permutation matrix p, */ +/* and the first n components of (q transpose)*b. on output */ +/* lmpar also provides an upper triangular matrix s such that */ + +/* t t t */ +/* p *(a *a + par*d*d)*p = s *s . */ + +/* s is employed within lmpar and may be of separate interest. */ + +/* only a few iterations are generally needed for convergence */ +/* of the algorithm. if, however, the limit of 10 iterations */ +/* is reached, then the output par will contain the best */ +/* value obtained so far. */ + +/* the subroutine statement is */ + +/* subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag, */ +/* wa1,wa2) */ + +/* where */ + +/* n is a positive integer input variable set to the order of r. */ + +/* r is an n by n array. on input the full upper triangle */ +/* must contain the full upper triangle of the matrix r. */ +/* on output the full upper triangle is unaltered, and the */ +/* strict lower triangle contains the strict upper triangle */ +/* (transposed) of the upper triangular matrix s. */ + +/* ldr is a positive integer input variable not less than n */ +/* which specifies the leading dimension of the array r. */ + +/* ipvt is an integer input array of length n which defines the */ +/* permutation matrix p such that a*p = q*r. column j of p */ +/* is column ipvt(j) of the identity matrix. */ + +/* diag is an input array of length n which must contain the */ +/* diagonal elements of the matrix d. */ + +/* qtb is an input array of length n which must contain the first */ + +/* n elements of the vector (q transpose)*b. */ + +/* delta is a positive input variable which specifies an upper */ +/* bound on the euclidean norm of d*x. */ + +/* par is a nonnegative variable. on input par contains an */ +/* initial estimate of the levenberg-marquardt parameter. */ +/* on output par contains the final estimate. */ + +/* x is an output array of length n which contains the least */ +/* squares solution of the system a*x = b, sqrt(par)*d*x = 0, */ +/* for the output par. */ + +/* sdiag is an output array of length n which contains the */ +/* diagonal elements of the upper triangular matrix s. */ + +/* wa1 and wa2 are work arrays of length n. */ + +/* subprograms called */ + +/* minpack-supplied ... dpmpar,enorm,qrsolv */ + +/* fortran-supplied ... dabs,dmax1,dmin1,dsqrt */ + +/* argonne national laboratory. minpack project. march 1980. */ +/* burton s. garbow, kenneth e. hillstrom, jorge j. more */ + +/* ********** */ + /* Parameter adjustments */ + --wa2; + --wa1; + --sdiag; + --x; + --qtb; + --diag; + --ipvt; + r_dim1 = *ldr; + r_offset = r_dim1 + 1; + r -= r_offset; + + /* Function Body */ + +/* dwarf is the smallest positive magnitude. */ + + dwarf = dpmpar_(&c__2); + +/* compute and store in x the gauss-newton direction. if the */ +/* jacobian is rank-deficient, obtain a least squares solution. */ + + nsing = *n; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + wa1[j] = qtb[j]; + if (r[j + j * r_dim1] == zero && nsing == *n) { + nsing = j - 1; + } + if (nsing < *n) { + wa1[j] = zero; + } +/* L10: */ + } + if (nsing < 1) { + goto L50; + } + i__1 = nsing; + for (k = 1; k <= i__1; ++k) { + j = nsing - k + 1; + wa1[j] /= r[j + j * r_dim1]; + temp = wa1[j]; + jm1 = j - 1; + if (jm1 < 1) { + goto L30; + } + i__2 = jm1; + for (i = 1; i <= i__2; ++i) { + wa1[i] -= r[i + j * r_dim1] * temp; +/* L20: */ + } +L30: +/* L40: */ + ; + } +L50: + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + l = ipvt[j]; + x[l] = wa1[j]; +/* L60: */ + } + +/* initialize the iteration counter. */ +/* evaluate the function at the origin, and test */ +/* for acceptance of the gauss-newton direction. */ + + iter = 0; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + wa2[j] = diag[j] * x[j]; +/* L70: */ + } + dxnorm = enorm_(n, &wa2[1]); + fp = dxnorm - *delta; + if (fp <= p1 * *delta) { + goto L220; + } + +/* if the jacobian is not rank deficient, the newton */ +/* step provides a lower bound, parl, for the zero of */ +/* the function. otherwise set this bound to zero. */ + + parl = zero; + if (nsing < *n) { + goto L120; + } + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + l = ipvt[j]; + wa1[j] = diag[l] * (wa2[l] / dxnorm); +/* L80: */ + } + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + sum = zero; + jm1 = j - 1; + if (jm1 < 1) { + goto L100; + } + i__2 = jm1; + for (i = 1; i <= i__2; ++i) { + sum += r[i + j * r_dim1] * wa1[i]; +/* L90: */ + } +L100: + wa1[j] = (wa1[j] - sum) / r[j + j * r_dim1]; +/* L110: */ + } + temp = enorm_(n, &wa1[1]); + parl = fp / *delta / temp / temp; +L120: + +/* calculate an upper bound, paru, for the zero of the function. */ + + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + sum = zero; + i__2 = j; + for (i = 1; i <= i__2; ++i) { + sum += r[i + j * r_dim1] * qtb[i]; +/* L130: */ + } + l = ipvt[j]; + wa1[j] = sum / diag[l]; +/* L140: */ + } + gnorm = enorm_(n, &wa1[1]); + paru = gnorm / *delta; + if (paru == zero) { + paru = dwarf / min(*delta,p1); + } + +/* if the input par lies outside of the interval (parl,paru), */ +/* set par to the closer endpoint. */ + + *par = max(*par,parl); + *par = min(*par,paru); + if (*par == zero) { + *par = gnorm / dxnorm; + } + +/* beginning of an iteration. */ + +L150: + ++iter; + +/* evaluate the function at the current value of par. */ + + if (*par == zero) { +/* Computing MAX */ + d__1 = dwarf, d__2 = p001 * paru; + *par = max(d__1,d__2); + } + temp = sqrt(*par); + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + wa1[j] = temp * diag[j]; +/* L160: */ + } + qrsolv_(n, &r[r_offset], ldr, &ipvt[1], &wa1[1], &qtb[1], &x[1], &sdiag[1] + , &wa2[1]); + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + wa2[j] = diag[j] * x[j]; +/* L170: */ + } + dxnorm = enorm_(n, &wa2[1]); + temp = fp; + fp = dxnorm - *delta; + +/* if the function is small enough, accept the current value */ +/* of par. also test for the exceptional cases where parl */ +/* is zero or the number of iterations has reached 10. */ + + if ((abs(fp) <= p1 * *delta) || (parl == zero && fp <= temp && temp < zero) || + (iter == 10)) { + goto L220; + } + +/* compute the newton correction. */ + + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + l = ipvt[j]; + wa1[j] = diag[l] * (wa2[l] / dxnorm); +/* L180: */ + } + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + wa1[j] /= sdiag[j]; + temp = wa1[j]; + jp1 = j + 1; + if (*n < jp1) { + goto L200; + } + i__2 = *n; + for (i = jp1; i <= i__2; ++i) { + wa1[i] -= r[i + j * r_dim1] * temp; +/* L190: */ + } +L200: +/* L210: */ + ; + } + temp = enorm_(n, &wa1[1]); + parc = fp / *delta / temp / temp; + +/* depending on the sign of the function, update parl or paru. */ + + if (fp > zero) { + parl = max(parl,*par); + } + if (fp < zero) { + paru = min(paru,*par); + } + +/* compute an improved estimate for par. */ + +/* Computing MAX */ + d__1 = parl, d__2 = *par + parc; + *par = max(d__1,d__2); + +/* end of an iteration. */ + + goto L150; +L220: + +/* termination. */ + + if (iter == 0) { + *par = zero; + } + return 0; + +/* last card of subroutine lmpar. */ + +} /* lmpar_ */ + diff --git a/others/pyTsai/src/minpack/lmpar.f b/others/pyTsai/src/minpack/lmpar.f new file mode 100644 index 0000000..26c422a --- /dev/null +++ b/others/pyTsai/src/minpack/lmpar.f @@ -0,0 +1,264 @@ + subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag,wa1, + * wa2) + integer n,ldr + integer ipvt(n) + double precision delta,par + double precision r(ldr,n),diag(n),qtb(n),x(n),sdiag(n),wa1(n), + * wa2(n) +c ********** +c +c subroutine lmpar +c +c given an m by n matrix a, an n by n nonsingular diagonal +c matrix d, an m-vector b, and a positive number delta, +c the problem is to determine a value for the parameter +c par such that if x solves the system +c +c a*x = b , sqrt(par)*d*x = 0 , +c +c in the least squares sense, and dxnorm is the euclidean +c norm of d*x, then either par is zero and +c +c (dxnorm-delta) .le. 0.1*delta , +c +c or par is positive and +c +c abs(dxnorm-delta) .le. 0.1*delta . +c +c this subroutine completes the solution of the problem +c if it is provided with the necessary information from the +c qr factorization, with column pivoting, of a. that is, if +c a*p = q*r, where p is a permutation matrix, q has orthogonal +c columns, and r is an upper triangular matrix with diagonal +c elements of nonincreasing magnitude, then lmpar expects +c the full upper triangle of r, the permutation matrix p, +c and the first n components of (q transpose)*b. on output +c lmpar also provides an upper triangular matrix s such that +c +c t t t +c p *(a *a + par*d*d)*p = s *s . +c +c s is employed within lmpar and may be of separate interest. +c +c only a few iterations are generally needed for convergence +c of the algorithm. if, however, the limit of 10 iterations +c is reached, then the output par will contain the best +c value obtained so far. +c +c the subroutine statement is +c +c subroutine lmpar(n,r,ldr,ipvt,diag,qtb,delta,par,x,sdiag, +c wa1,wa2) +c +c where +c +c n is a positive integer input variable set to the order of r. +c +c r is an n by n array. on input the full upper triangle +c must contain the full upper triangle of the matrix r. +c on output the full upper triangle is unaltered, and the +c strict lower triangle contains the strict upper triangle +c (transposed) of the upper triangular matrix s. +c +c ldr is a positive integer input variable not less than n +c which specifies the leading dimension of the array r. +c +c ipvt is an integer input array of length n which defines the +c permutation matrix p such that a*p = q*r. column j of p +c is column ipvt(j) of the identity matrix. +c +c diag is an input array of length n which must contain the +c diagonal elements of the matrix d. +c +c qtb is an input array of length n which must contain the first +c n elements of the vector (q transpose)*b. +c +c delta is a positive input variable which specifies an upper +c bound on the euclidean norm of d*x. +c +c par is a nonnegative variable. on input par contains an +c initial estimate of the levenberg-marquardt parameter. +c on output par contains the final estimate. +c +c x is an output array of length n which contains the least +c squares solution of the system a*x = b, sqrt(par)*d*x = 0, +c for the output par. +c +c sdiag is an output array of length n which contains the +c diagonal elements of the upper triangular matrix s. +c +c wa1 and wa2 are work arrays of length n. +c +c subprograms called +c +c minpack-supplied ... dpmpar,enorm,qrsolv +c +c fortran-supplied ... dabs,dmax1,dmin1,dsqrt +c +c argonne national laboratory. minpack project. march 1980. +c burton s. garbow, kenneth e. hillstrom, jorge j. more +c +c ********** + integer i,iter,j,jm1,jp1,k,l,nsing + double precision dxnorm,dwarf,fp,gnorm,parc,parl,paru,p1,p001, + * sum,temp,zero + double precision dpmpar,enorm + data p1,p001,zero /1.0d-1,1.0d-3,0.0d0/ +c +c dwarf is the smallest positive magnitude. +c + dwarf = dpmpar(2) +c +c compute and store in x the gauss-newton direction. if the +c jacobian is rank-deficient, obtain a least squares solution. +c + nsing = n + do 10 j = 1, n + wa1(j) = qtb(j) + if (r(j,j) .eq. zero .and. nsing .eq. n) nsing = j - 1 + if (nsing .lt. n) wa1(j) = zero + 10 continue + if (nsing .lt. 1) go to 50 + do 40 k = 1, nsing + j = nsing - k + 1 + wa1(j) = wa1(j)/r(j,j) + temp = wa1(j) + jm1 = j - 1 + if (jm1 .lt. 1) go to 30 + do 20 i = 1, jm1 + wa1(i) = wa1(i) - r(i,j)*temp + 20 continue + 30 continue + 40 continue + 50 continue + do 60 j = 1, n + l = ipvt(j) + x(l) = wa1(j) + 60 continue +c +c initialize the iteration counter. +c evaluate the function at the origin, and test +c for acceptance of the gauss-newton direction. +c + iter = 0 + do 70 j = 1, n + wa2(j) = diag(j)*x(j) + 70 continue + dxnorm = enorm(n,wa2) + fp = dxnorm - delta + if (fp .le. p1*delta) go to 220 +c +c if the jacobian is not rank deficient, the newton +c step provides a lower bound, parl, for the zero of +c the function. otherwise set this bound to zero. +c + parl = zero + if (nsing .lt. n) go to 120 + do 80 j = 1, n + l = ipvt(j) + wa1(j) = diag(l)*(wa2(l)/dxnorm) + 80 continue + do 110 j = 1, n + sum = zero + jm1 = j - 1 + if (jm1 .lt. 1) go to 100 + do 90 i = 1, jm1 + sum = sum + r(i,j)*wa1(i) + 90 continue + 100 continue + wa1(j) = (wa1(j) - sum)/r(j,j) + 110 continue + temp = enorm(n,wa1) + parl = ((fp/delta)/temp)/temp + 120 continue +c +c calculate an upper bound, paru, for the zero of the function. +c + do 140 j = 1, n + sum = zero + do 130 i = 1, j + sum = sum + r(i,j)*qtb(i) + 130 continue + l = ipvt(j) + wa1(j) = sum/diag(l) + 140 continue + gnorm = enorm(n,wa1) + paru = gnorm/delta + if (paru .eq. zero) paru = dwarf/dmin1(delta,p1) +c +c if the input par lies outside of the interval (parl,paru), +c set par to the closer endpoint. +c + par = dmax1(par,parl) + par = dmin1(par,paru) + if (par .eq. zero) par = gnorm/dxnorm +c +c beginning of an iteration. +c + 150 continue + iter = iter + 1 +c +c evaluate the function at the current value of par. +c + if (par .eq. zero) par = dmax1(dwarf,p001*paru) + temp = dsqrt(par) + do 160 j = 1, n + wa1(j) = temp*diag(j) + 160 continue + call qrsolv(n,r,ldr,ipvt,wa1,qtb,x,sdiag,wa2) + do 170 j = 1, n + wa2(j) = diag(j)*x(j) + 170 continue + dxnorm = enorm(n,wa2) + temp = fp + fp = dxnorm - delta +c +c if the function is small enough, accept the current value +c of par. also test for the exceptional cases where parl +c is zero or the number of iterations has reached 10. +c + if (dabs(fp) .le. p1*delta + * .or. parl .eq. zero .and. fp .le. temp + * .and. temp .lt. zero .or. iter .eq. 10) go to 220 +c +c compute the newton correction. +c + do 180 j = 1, n + l = ipvt(j) + wa1(j) = diag(l)*(wa2(l)/dxnorm) + 180 continue + do 210 j = 1, n + wa1(j) = wa1(j)/sdiag(j) + temp = wa1(j) + jp1 = j + 1 + if (n .lt. jp1) go to 200 + do 190 i = jp1, n + wa1(i) = wa1(i) - r(i,j)*temp + 190 continue + 200 continue + 210 continue + temp = enorm(n,wa1) + parc = ((fp/delta)/temp)/temp +c +c depending on the sign of the function, update parl or paru. +c + if (fp .gt. zero) parl = dmax1(parl,par) + if (fp .lt. zero) paru = dmin1(paru,par) +c +c compute an improved estimate for par. +c + par = dmax1(parl,par+parc) +c +c end of an iteration. +c + go to 150 + 220 continue +c +c termination. +c + if (iter .eq. 0) par = zero + return +c +c last card of subroutine lmpar. +c + end diff --git a/others/pyTsai/src/minpack/minpack.h b/others/pyTsai/src/minpack/minpack.h new file mode 100644 index 0000000..12b105f --- /dev/null +++ b/others/pyTsai/src/minpack/minpack.h @@ -0,0 +1,6 @@ +/** + * Function declarations for methods used by the camera calibration code within + * the minpack library. + */ + +int lmdif_(); diff --git a/others/pyTsai/src/minpack/qrfac.c b/others/pyTsai/src/minpack/qrfac.c new file mode 100644 index 0000000..ebb85cb --- /dev/null +++ b/others/pyTsai/src/minpack/qrfac.c @@ -0,0 +1,255 @@ +/* qrfac.f -- translated by f2c (version of 17 January 1992 0:17:58). + You must link the resulting object file with the libraries: + -lf77 -li77 -lm -lc (in that order) +*/ + +#include "f2c.h" + +/* Table of constant values */ + +static integer c__1 = 1; + +/* Subroutine */ int qrfac_(m, n, a, lda, pivot, ipvt, lipvt, rdiag, acnorm, + wa) +integer *m, *n; +doublereal *a; +integer *lda; +logical *pivot; +integer *ipvt, *lipvt; +doublereal *rdiag, *acnorm, *wa; +{ + /* Initialized data */ + + static doublereal one = 1.; + static doublereal p05 = .05; + static doublereal zero = 0.; + + /* System generated locals */ + integer a_dim1, a_offset, i__1, i__2, i__3; + doublereal d__1, d__2, d__3; + + /* Builtin functions */ + double sqrt(); + + /* Local variables */ + static integer kmax; + static doublereal temp; + static integer i, j, k, minmn; + extern doublereal enorm_(); + static doublereal epsmch; + extern doublereal dpmpar_(); + static doublereal ajnorm; + static integer jp1; + static doublereal sum; + +/* ********** */ + +/* subroutine qrfac */ + +/* this subroutine uses householder transformations with column */ +/* pivoting (optional) to compute a qr factorization of the */ +/* m by n matrix a. that is, qrfac determines an orthogonal */ +/* matrix q, a permutation matrix p, and an upper trapezoidal */ +/* matrix r with diagonal elements of nonincreasing magnitude, */ +/* such that a*p = q*r. the householder transformation for */ +/* column k, k = 1,2,...,min(m,n), is of the form */ + +/* t */ +/* i - (1/u(k))*u*u */ + +/* where u has zeros in the first k-1 positions. the form of */ +/* this transformation and the method of pivoting first */ +/* appeared in the corresponding linpack subroutine. */ + +/* the subroutine statement is */ + +/* subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa) */ + +/* where */ + +/* m is a positive integer input variable set to the number */ +/* of rows of a. */ + +/* n is a positive integer input variable set to the number */ +/* of columns of a. */ + +/* a is an m by n array. on input a contains the matrix for */ +/* which the qr factorization is to be computed. on output */ +/* the strict upper trapezoidal part of a contains the strict */ +/* upper trapezoidal part of r, and the lower trapezoidal */ +/* part of a contains a factored form of q (the non-trivial */ +/* elements of the u vectors described above). */ + +/* lda is a positive integer input variable not less than m */ +/* which specifies the leading dimension of the array a. */ + +/* pivot is a logical input variable. if pivot is set true, */ +/* then column pivoting is enforced. if pivot is set false, */ +/* then no column pivoting is done. */ + +/* ipvt is an integer output array of length lipvt. ipvt */ +/* defines the permutation matrix p such that a*p = q*r. */ +/* column j of p is column ipvt(j) of the identity matrix. */ +/* if pivot is false, ipvt is not referenced. */ + +/* lipvt is a positive integer input variable. if pivot is false, */ + +/* then lipvt may be as small as 1. if pivot is true, then */ +/* lipvt must be at least n. */ + +/* rdiag is an output array of length n which contains the */ +/* diagonal elements of r. */ + +/* acnorm is an output array of length n which contains the */ +/* norms of the corresponding columns of the input matrix a. */ +/* if this information is not needed, then acnorm can coincide */ +/* with rdiag. */ + +/* wa is a work array of length n. if pivot is false, then wa */ +/* can coincide with rdiag. */ + +/* subprograms called */ + +/* minpack-supplied ... dpmpar,enorm */ + +/* fortran-supplied ... dmax1,dsqrt,min0 */ + +/* argonne national laboratory. minpack project. march 1980. */ +/* burton s. garbow, kenneth e. hillstrom, jorge j. more */ + +/* ********** */ + /* Parameter adjustments */ + --wa; + --acnorm; + --rdiag; + --ipvt; + a_dim1 = *lda; + a_offset = a_dim1 + 1; + a -= a_offset; + + /* Function Body */ + +/* epsmch is the machine precision. */ + + epsmch = dpmpar_(&c__1); + +/* compute the initial column norms and initialize several arrays. */ + + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + acnorm[j] = enorm_(m, &a[j * a_dim1 + 1]); + rdiag[j] = acnorm[j]; + wa[j] = rdiag[j]; + if (*pivot) { + ipvt[j] = j; + } +/* L10: */ + } + +/* reduce a to r with householder transformations. */ + + minmn = min(*m,*n); + i__1 = minmn; + for (j = 1; j <= i__1; ++j) { + if (! (*pivot)) { + goto L40; + } + +/* bring the column of largest norm into the pivot position. */ + + + kmax = j; + i__2 = *n; + for (k = j; k <= i__2; ++k) { + if (rdiag[k] > rdiag[kmax]) { + kmax = k; + } +/* L20: */ + } + if (kmax == j) { + goto L40; + } + i__2 = *m; + for (i = 1; i <= i__2; ++i) { + temp = a[i + j * a_dim1]; + a[i + j * a_dim1] = a[i + kmax * a_dim1]; + a[i + kmax * a_dim1] = temp; +/* L30: */ + } + rdiag[kmax] = rdiag[j]; + wa[kmax] = wa[j]; + k = ipvt[j]; + ipvt[j] = ipvt[kmax]; + ipvt[kmax] = k; +L40: + +/* compute the householder transformation to reduce the */ +/* j-th column of a to a multiple of the j-th unit vector. */ + + i__2 = *m - j + 1; + ajnorm = enorm_(&i__2, &a[j + j * a_dim1]); + if (ajnorm == zero) { + goto L100; + } + if (a[j + j * a_dim1] < zero) { + ajnorm = -ajnorm; + } + i__2 = *m; + for (i = j; i <= i__2; ++i) { + a[i + j * a_dim1] /= ajnorm; +/* L50: */ + } + a[j + j * a_dim1] += one; + +/* apply the transformation to the remaining columns */ +/* and update the norms. */ + + jp1 = j + 1; + if (*n < jp1) { + goto L100; + } + i__2 = *n; + for (k = jp1; k <= i__2; ++k) { + sum = zero; + i__3 = *m; + for (i = j; i <= i__3; ++i) { + sum += a[i + j * a_dim1] * a[i + k * a_dim1]; +/* L60: */ + } + temp = sum / a[j + j * a_dim1]; + i__3 = *m; + for (i = j; i <= i__3; ++i) { + a[i + k * a_dim1] -= temp * a[i + j * a_dim1]; +/* L70: */ + } + if (! (*pivot) || rdiag[k] == zero) { + goto L80; + } + temp = a[j + k * a_dim1] / rdiag[k]; +/* Computing MAX */ +/* Computing 2nd power */ + d__3 = temp; + d__1 = zero, d__2 = one - d__3 * d__3; + rdiag[k] *= sqrt((max(d__1,d__2))); +/* Computing 2nd power */ + d__1 = rdiag[k] / wa[k]; + if (p05 * (d__1 * d__1) > epsmch) { + goto L80; + } + i__3 = *m - j; + rdiag[k] = enorm_(&i__3, &a[jp1 + k * a_dim1]); + wa[k] = rdiag[k]; +L80: +/* L90: */ + ; + } +L100: + rdiag[j] = -ajnorm; +/* L110: */ + } + return 0; + +/* last card of subroutine qrfac. */ + +} /* qrfac_ */ + diff --git a/others/pyTsai/src/minpack/qrfac.f b/others/pyTsai/src/minpack/qrfac.f new file mode 100644 index 0000000..cb68608 --- /dev/null +++ b/others/pyTsai/src/minpack/qrfac.f @@ -0,0 +1,164 @@ + subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa) + integer m,n,lda,lipvt + integer ipvt(lipvt) + logical pivot + double precision a(lda,n),rdiag(n),acnorm(n),wa(n) +c ********** +c +c subroutine qrfac +c +c this subroutine uses householder transformations with column +c pivoting (optional) to compute a qr factorization of the +c m by n matrix a. that is, qrfac determines an orthogonal +c matrix q, a permutation matrix p, and an upper trapezoidal +c matrix r with diagonal elements of nonincreasing magnitude, +c such that a*p = q*r. the householder transformation for +c column k, k = 1,2,...,min(m,n), is of the form +c +c t +c i - (1/u(k))*u*u +c +c where u has zeros in the first k-1 positions. the form of +c this transformation and the method of pivoting first +c appeared in the corresponding linpack subroutine. +c +c the subroutine statement is +c +c subroutine qrfac(m,n,a,lda,pivot,ipvt,lipvt,rdiag,acnorm,wa) +c +c where +c +c m is a positive integer input variable set to the number +c of rows of a. +c +c n is a positive integer input variable set to the number +c of columns of a. +c +c a is an m by n array. on input a contains the matrix for +c which the qr factorization is to be computed. on output +c the strict upper trapezoidal part of a contains the strict +c upper trapezoidal part of r, and the lower trapezoidal +c part of a contains a factored form of q (the non-trivial +c elements of the u vectors described above). +c +c lda is a positive integer input variable not less than m +c which specifies the leading dimension of the array a. +c +c pivot is a logical input variable. if pivot is set true, +c then column pivoting is enforced. if pivot is set false, +c then no column pivoting is done. +c +c ipvt is an integer output array of length lipvt. ipvt +c defines the permutation matrix p such that a*p = q*r. +c column j of p is column ipvt(j) of the identity matrix. +c if pivot is false, ipvt is not referenced. +c +c lipvt is a positive integer input variable. if pivot is false, +c then lipvt may be as small as 1. if pivot is true, then +c lipvt must be at least n. +c +c rdiag is an output array of length n which contains the +c diagonal elements of r. +c +c acnorm is an output array of length n which contains the +c norms of the corresponding columns of the input matrix a. +c if this information is not needed, then acnorm can coincide +c with rdiag. +c +c wa is a work array of length n. if pivot is false, then wa +c can coincide with rdiag. +c +c subprograms called +c +c minpack-supplied ... dpmpar,enorm +c +c fortran-supplied ... dmax1,dsqrt,min0 +c +c argonne national laboratory. minpack project. march 1980. +c burton s. garbow, kenneth e. hillstrom, jorge j. more +c +c ********** + integer i,j,jp1,k,kmax,minmn + double precision ajnorm,epsmch,one,p05,sum,temp,zero + double precision dpmpar,enorm + data one,p05,zero /1.0d0,5.0d-2,0.0d0/ +c +c epsmch is the machine precision. +c + epsmch = dpmpar(1) +c +c compute the initial column norms and initialize several arrays. +c + do 10 j = 1, n + acnorm(j) = enorm(m,a(1,j)) + rdiag(j) = acnorm(j) + wa(j) = rdiag(j) + if (pivot) ipvt(j) = j + 10 continue +c +c reduce a to r with householder transformations. +c + minmn = min0(m,n) + do 110 j = 1, minmn + if (.not.pivot) go to 40 +c +c bring the column of largest norm into the pivot position. +c + kmax = j + do 20 k = j, n + if (rdiag(k) .gt. rdiag(kmax)) kmax = k + 20 continue + if (kmax .eq. j) go to 40 + do 30 i = 1, m + temp = a(i,j) + a(i,j) = a(i,kmax) + a(i,kmax) = temp + 30 continue + rdiag(kmax) = rdiag(j) + wa(kmax) = wa(j) + k = ipvt(j) + ipvt(j) = ipvt(kmax) + ipvt(kmax) = k + 40 continue +c +c compute the householder transformation to reduce the +c j-th column of a to a multiple of the j-th unit vector. +c + ajnorm = enorm(m-j+1,a(j,j)) + if (ajnorm .eq. zero) go to 100 + if (a(j,j) .lt. zero) ajnorm = -ajnorm + do 50 i = j, m + a(i,j) = a(i,j)/ajnorm + 50 continue + a(j,j) = a(j,j) + one +c +c apply the transformation to the remaining columns +c and update the norms. +c + jp1 = j + 1 + if (n .lt. jp1) go to 100 + do 90 k = jp1, n + sum = zero + do 60 i = j, m + sum = sum + a(i,j)*a(i,k) + 60 continue + temp = sum/a(j,j) + do 70 i = j, m + a(i,k) = a(i,k) - temp*a(i,j) + 70 continue + if (.not.pivot .or. rdiag(k) .eq. zero) go to 80 + temp = a(j,k)/rdiag(k) + rdiag(k) = rdiag(k)*dsqrt(dmax1(zero,one-temp**2)) + if (p05*(rdiag(k)/wa(k))**2 .gt. epsmch) go to 80 + rdiag(k) = enorm(m-j,a(jp1,k)) + wa(k) = rdiag(k) + 80 continue + 90 continue + 100 continue + rdiag(j) = -ajnorm + 110 continue + return +c +c last card of subroutine qrfac. +c + end diff --git a/others/pyTsai/src/minpack/qrsolv.c b/others/pyTsai/src/minpack/qrsolv.c new file mode 100644 index 0000000..0f1e870 --- /dev/null +++ b/others/pyTsai/src/minpack/qrsolv.c @@ -0,0 +1,277 @@ +/* qrsolv.f -- translated by f2c (version of 17 January 1992 0:17:58). + You must link the resulting object file with the libraries: + -lf77 -li77 -lm -lc (in that order) +*/ + +#include "f2c.h" + +/* Subroutine */ int qrsolv_(n, r, ldr, ipvt, diag, qtb, x, sdiag, wa) +integer *n; +doublereal *r; +integer *ldr, *ipvt; +doublereal *diag, *qtb, *x, *sdiag, *wa; +{ + /* Initialized data */ + + static doublereal p5 = .5; + static doublereal p25 = .25; + static doublereal zero = 0.; + + /* System generated locals */ + integer r_dim1, r_offset, i__1, i__2, i__3; + doublereal d__1, d__2; + + /* Builtin functions */ + double sqrt(); + + /* Local variables */ + static doublereal temp; + static integer i, j, k, l; + static doublereal cotan; + static integer nsing; + static doublereal qtbpj; + static integer jp1, kp1; + static doublereal tan_, cos_, sin_, sum; + +/* ********** */ + +/* subroutine qrsolv */ + +/* given an m by n matrix a, an n by n diagonal matrix d, */ +/* and an m-vector b, the problem is to determine an x which */ +/* solves the system */ + +/* a*x = b , d*x = 0 , */ + +/* in the least squares sense. */ + +/* this subroutine completes the solution of the problem */ +/* if it is provided with the necessary information from the */ +/* qr factorization, with column pivoting, of a. that is, if */ +/* a*p = q*r, where p is a permutation matrix, q has orthogonal */ +/* columns, and r is an upper triangular matrix with diagonal */ +/* elements of nonincreasing magnitude, then qrsolv expects */ +/* the full upper triangle of r, the permutation matrix p, */ +/* and the first n components of (q transpose)*b. the system */ +/* a*x = b, d*x = 0, is then equivalent to */ + +/* t t */ +/* r*z = q *b , p *d*p*z = 0 , */ + +/* where x = p*z. if this system does not have full rank, */ +/* then a least squares solution is obtained. on output qrsolv */ +/* also provides an upper triangular matrix s such that */ + +/* t t t */ +/* p *(a *a + d*d)*p = s *s . */ + +/* s is computed within qrsolv and may be of separate interest. */ + +/* the subroutine statement is */ + +/* subroutine qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa) */ + +/* where */ + +/* n is a positive integer input variable set to the order of r. */ + +/* r is an n by n array. on input the full upper triangle */ +/* must contain the full upper triangle of the matrix r. */ +/* on output the full upper triangle is unaltered, and the */ +/* strict lower triangle contains the strict upper triangle */ +/* (transposed) of the upper triangular matrix s. */ + +/* ldr is a positive integer input variable not less than n */ +/* which specifies the leading dimension of the array r. */ + +/* ipvt is an integer input array of length n which defines the */ +/* permutation matrix p such that a*p = q*r. column j of p */ +/* is column ipvt(j) of the identity matrix. */ + +/* diag is an input array of length n which must contain the */ +/* diagonal elements of the matrix d. */ + +/* qtb is an input array of length n which must contain the first */ + +/* n elements of the vector (q transpose)*b. */ + +/* x is an output array of length n which contains the least */ +/* squares solution of the system a*x = b, d*x = 0. */ + +/* sdiag is an output array of length n which contains the */ +/* diagonal elements of the upper triangular matrix s. */ + +/* wa is a work array of length n. */ + +/* subprograms called */ + +/* fortran-supplied ... dabs,dsqrt */ + +/* argonne national laboratory. minpack project. march 1980. */ +/* burton s. garbow, kenneth e. hillstrom, jorge j. more */ + +/* ********** */ + /* Parameter adjustments */ + --wa; + --sdiag; + --x; + --qtb; + --diag; + --ipvt; + r_dim1 = *ldr; + r_offset = r_dim1 + 1; + r -= r_offset; + + /* Function Body */ + +/* copy r and (q transpose)*b to preserve input and initialize s. */ +/* in particular, save the diagonal elements of r in x. */ + + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + i__2 = *n; + for (i = j; i <= i__2; ++i) { + r[i + j * r_dim1] = r[j + i * r_dim1]; +/* L10: */ + } + x[j] = r[j + j * r_dim1]; + wa[j] = qtb[j]; +/* L20: */ + } + +/* eliminate the diagonal matrix d using a givens rotation. */ + + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + +/* prepare the row of d to be eliminated, locating the */ +/* diagonal element using p from the qr factorization. */ + + l = ipvt[j]; + if (diag[l] == zero) { + goto L90; + } + i__2 = *n; + for (k = j; k <= i__2; ++k) { + sdiag[k] = zero; +/* L30: */ + } + sdiag[j] = diag[l]; + +/* the transformations to eliminate the row of d */ +/* modify only a single element of (q transpose)*b */ +/* beyond the first n, which is initially zero. */ + + qtbpj = zero; + i__2 = *n; + for (k = j; k <= i__2; ++k) { + +/* determine a givens rotation which eliminates the */ +/* appropriate element in the current row of d. */ + + if (sdiag[k] == zero) { + goto L70; + } + if ((d__1 = r[k + k * r_dim1], abs(d__1)) >= (d__2 = sdiag[k], + abs(d__2))) { + goto L40; + } + cotan = r[k + k * r_dim1] / sdiag[k]; +/* Computing 2nd power */ + d__1 = cotan; + sin_ = p5 / sqrt(p25 + p25 * (d__1 * d__1)); + cos_ = sin_ * cotan; + goto L50; +L40: + tan_ = sdiag[k] / r[k + k * r_dim1]; +/* Computing 2nd power */ + d__1 = tan_; + cos_ = p5 / sqrt(p25 + p25 * (d__1 * d__1)); + sin_ = cos_ * tan_; +L50: + +/* compute the modified diagonal element of r and */ +/* the modified element of ((q transpose)*b,0). */ + + r[k + k * r_dim1] = cos_ * r[k + k * r_dim1] + sin_ * sdiag[k]; + temp = cos_ * wa[k] + sin_ * qtbpj; + qtbpj = -sin_ * wa[k] + cos_ * qtbpj; + wa[k] = temp; + +/* accumulate the tranformation in the row of s. */ + + kp1 = k + 1; + if (*n < kp1) { + goto L70; + } + i__3 = *n; + for (i = kp1; i <= i__3; ++i) { + temp = cos_ * r[i + k * r_dim1] + sin_ * sdiag[i]; + sdiag[i] = -sin_ * r[i + k * r_dim1] + cos_ * sdiag[i]; + r[i + k * r_dim1] = temp; +/* L60: */ + } +L70: +/* L80: */ + ; + } +L90: + +/* store the diagonal element of s and restore */ +/* the corresponding diagonal element of r. */ + + sdiag[j] = r[j + j * r_dim1]; + r[j + j * r_dim1] = x[j]; +/* L100: */ + } + +/* solve the triangular system for z. if the system is */ +/* singular, then obtain a least squares solution. */ + + nsing = *n; + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + if (sdiag[j] == zero && nsing == *n) { + nsing = j - 1; + } + if (nsing < *n) { + wa[j] = zero; + } +/* L110: */ + } + if (nsing < 1) { + goto L150; + } + i__1 = nsing; + for (k = 1; k <= i__1; ++k) { + j = nsing - k + 1; + sum = zero; + jp1 = j + 1; + if (nsing < jp1) { + goto L130; + } + i__2 = nsing; + for (i = jp1; i <= i__2; ++i) { + sum += r[i + j * r_dim1] * wa[i]; +/* L120: */ + } +L130: + wa[j] = (wa[j] - sum) / sdiag[j]; +/* L140: */ + } +L150: + +/* permute the components of z back to components of x. */ + + i__1 = *n; + for (j = 1; j <= i__1; ++j) { + l = ipvt[j]; + x[l] = wa[j]; +/* L160: */ + } + return 0; + +/* last card of subroutine qrsolv. */ + +} /* qrsolv_ */ + diff --git a/others/pyTsai/src/minpack/qrsolv.f b/others/pyTsai/src/minpack/qrsolv.f new file mode 100644 index 0000000..f48954b --- /dev/null +++ b/others/pyTsai/src/minpack/qrsolv.f @@ -0,0 +1,193 @@ + subroutine qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa) + integer n,ldr + integer ipvt(n) + double precision r(ldr,n),diag(n),qtb(n),x(n),sdiag(n),wa(n) +c ********** +c +c subroutine qrsolv +c +c given an m by n matrix a, an n by n diagonal matrix d, +c and an m-vector b, the problem is to determine an x which +c solves the system +c +c a*x = b , d*x = 0 , +c +c in the least squares sense. +c +c this subroutine completes the solution of the problem +c if it is provided with the necessary information from the +c qr factorization, with column pivoting, of a. that is, if +c a*p = q*r, where p is a permutation matrix, q has orthogonal +c columns, and r is an upper triangular matrix with diagonal +c elements of nonincreasing magnitude, then qrsolv expects +c the full upper triangle of r, the permutation matrix p, +c and the first n components of (q transpose)*b. the system +c a*x = b, d*x = 0, is then equivalent to +c +c t t +c r*z = q *b , p *d*p*z = 0 , +c +c where x = p*z. if this system does not have full rank, +c then a least squares solution is obtained. on output qrsolv +c also provides an upper triangular matrix s such that +c +c t t t +c p *(a *a + d*d)*p = s *s . +c +c s is computed within qrsolv and may be of separate interest. +c +c the subroutine statement is +c +c subroutine qrsolv(n,r,ldr,ipvt,diag,qtb,x,sdiag,wa) +c +c where +c +c n is a positive integer input variable set to the order of r. +c +c r is an n by n array. on input the full upper triangle +c must contain the full upper triangle of the matrix r. +c on output the full upper triangle is unaltered, and the +c strict lower triangle contains the strict upper triangle +c (transposed) of the upper triangular matrix s. +c +c ldr is a positive integer input variable not less than n +c which specifies the leading dimension of the array r. +c +c ipvt is an integer input array of length n which defines the +c permutation matrix p such that a*p = q*r. column j of p +c is column ipvt(j) of the identity matrix. +c +c diag is an input array of length n which must contain the +c diagonal elements of the matrix d. +c +c qtb is an input array of length n which must contain the first +c n elements of the vector (q transpose)*b. +c +c x is an output array of length n which contains the least +c squares solution of the system a*x = b, d*x = 0. +c +c sdiag is an output array of length n which contains the +c diagonal elements of the upper triangular matrix s. +c +c wa is a work array of length n. +c +c subprograms called +c +c fortran-supplied ... dabs,dsqrt +c +c argonne national laboratory. minpack project. march 1980. +c burton s. garbow, kenneth e. hillstrom, jorge j. more +c +c ********** + integer i,j,jp1,k,kp1,l,nsing + double precision cos,cotan,p5,p25,qtbpj,sin,sum,tan,temp,zero + data p5,p25,zero /5.0d-1,2.5d-1,0.0d0/ +c +c copy r and (q transpose)*b to preserve input and initialize s. +c in particular, save the diagonal elements of r in x. +c + do 20 j = 1, n + do 10 i = j, n + r(i,j) = r(j,i) + 10 continue + x(j) = r(j,j) + wa(j) = qtb(j) + 20 continue +c +c eliminate the diagonal matrix d using a givens rotation. +c + do 100 j = 1, n +c +c prepare the row of d to be eliminated, locating the +c diagonal element using p from the qr factorization. +c + l = ipvt(j) + if (diag(l) .eq. zero) go to 90 + do 30 k = j, n + sdiag(k) = zero + 30 continue + sdiag(j) = diag(l) +c +c the transformations to eliminate the row of d +c modify only a single element of (q transpose)*b +c beyond the first n, which is initially zero. +c + qtbpj = zero + do 80 k = j, n +c +c determine a givens rotation which eliminates the +c appropriate element in the current row of d. +c + if (sdiag(k) .eq. zero) go to 70 + if (dabs(r(k,k)) .ge. dabs(sdiag(k))) go to 40 + cotan = r(k,k)/sdiag(k) + sin = p5/dsqrt(p25+p25*cotan**2) + cos = sin*cotan + go to 50 + 40 continue + tan = sdiag(k)/r(k,k) + cos = p5/dsqrt(p25+p25*tan**2) + sin = cos*tan + 50 continue +c +c compute the modified diagonal element of r and +c the modified element of ((q transpose)*b,0). +c + r(k,k) = cos*r(k,k) + sin*sdiag(k) + temp = cos*wa(k) + sin*qtbpj + qtbpj = -sin*wa(k) + cos*qtbpj + wa(k) = temp +c +c accumulate the tranformation in the row of s. +c + kp1 = k + 1 + if (n .lt. kp1) go to 70 + do 60 i = kp1, n + temp = cos*r(i,k) + sin*sdiag(i) + sdiag(i) = -sin*r(i,k) + cos*sdiag(i) + r(i,k) = temp + 60 continue + 70 continue + 80 continue + 90 continue +c +c store the diagonal element of s and restore +c the corresponding diagonal element of r. +c + sdiag(j) = r(j,j) + r(j,j) = x(j) + 100 continue +c +c solve the triangular system for z. if the system is +c singular, then obtain a least squares solution. +c + nsing = n + do 110 j = 1, n + if (sdiag(j) .eq. zero .and. nsing .eq. n) nsing = j - 1 + if (nsing .lt. n) wa(j) = zero + 110 continue + if (nsing .lt. 1) go to 150 + do 140 k = 1, nsing + j = nsing - k + 1 + sum = zero + jp1 = j + 1 + if (nsing .lt. jp1) go to 130 + do 120 i = jp1, nsing + sum = sum + r(i,j)*wa(i) + 120 continue + 130 continue + wa(j) = (wa(j) - sum)/sdiag(j) + 140 continue + 150 continue +c +c permute the components of z back to components of x. +c + do 160 j = 1, n + l = ipvt(j) + x(l) = wa(j) + 160 continue + return +c +c last card of subroutine qrsolv. +c + end diff --git a/others/pyTsai/src/pytsai-py2.c b/others/pyTsai/src/pytsai-py2.c new file mode 100644 index 0000000..cb62fc3 --- /dev/null +++ b/others/pyTsai/src/pytsai-py2.c @@ -0,0 +1,705 @@ +/** + * pytsai.c + * Copyright (C) Jonathan Merritt 2004. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#include "Python.h" +#include "tsai/cal_main.h" +#include "errors.h" + +/************************************* + * Forward Declarations of Functions * + *************************************/ +PyMODINIT_FUNC initpytsai(void); +static double* parse_calibration_data(PyObject *pyobj, int *size); +static int parse_camera_mapping(PyObject *obj); +static PyObject* build_camera_mapping(); +static PyObject* tsai_coplanar_calibration(PyObject *self, PyObject *args); +static PyObject* tsai_noncoplanar_calibration(PyObject *self, PyObject *args); +static PyObject* tsai_coplanar_calibration_fo(PyObject *self, PyObject *args); +static PyObject* tsai_noncoplanar_calibration_fo(PyObject *self, + PyObject *args); +static PyObject* tsai_wc2ic(PyObject *self, PyObject *args); +static PyObject* tsai_ic2wc(PyObject *self, PyObject *args); +static PyObject* tsai_cc2wc(PyObject *self, PyObject *args); +static PyObject* tsai_add_sensor_coord_distortion(PyObject *self, + PyObject *args); + +/*********************** + * Module Method Table * + ***********************/ +static PyMethodDef TsaiMethods[] = +{ + + {"_pytsai_coplanar_calibration", tsai_coplanar_calibration, + METH_VARARGS, "Low level coplanar calibration routine."}, + + {"_pytsai_noncoplanar_calibration", tsai_noncoplanar_calibration, + METH_VARARGS, "Low level non-coplanar calibration routine."}, + + {"_pytsai_coplanar_calibration_fo", tsai_coplanar_calibration_fo, + METH_VARARGS, + "Low level coplanar, with full optimization calibration routine."}, + + {"_pytsai_noncoplanar_calibration_fo", tsai_noncoplanar_calibration_fo, + METH_VARARGS, + "Low level noncoplanar, with full optimization calibration routine."}, + + {"_pytsai_wc2ic", tsai_wc2ic, METH_VARARGS, + "Low level conversion of world coordinates to image coordinates."}, + + {"_pytsai_ic2wc", tsai_ic2wc, METH_VARARGS, + "Low level conversion of image coordinates to world coordinates."}, + + {"_pytsai_cc2wc", tsai_cc2wc, METH_VARARGS, + "Low level conversion of camera coordinates to world coordinates."}, + + {"_pytsai_add_sensor_coord_distortion", + tsai_add_sensor_coord_distortion, METH_VARARGS, + "Low level conversion of undistorted to distorted image coordinates."}, + + {NULL, NULL, 0, NULL} + +}; + +/**************************** + * Function Implementations * + ****************************/ + +PyMODINIT_FUNC initpytsai(void) +{ + (void) Py_InitModule("pytsai", TsaiMethods); +} + +/** + * Parses calibration data. The calibration data should be in the form of a + * sequence of sequences. eg: + * [ + * [ xs, ys, zs, xi, yi ], ... + * ] + * where (xs, ys, zs) are the coordinates of a point in 3D space, and (xi, yi) + * are the coordinates of the corresponding point in an image. + * + * If the method fails, it returns NULL and raises an appropriate exception. + * On success, a flat array of doubles will be PyMem_Malloc()'d and filled + * with the calibration data: + * { xs1, ys1, zs1, xi1, yi1, xs2, ys2, zs2, xi2, yi2, ... } + * size will be populated with the number of calibration points. + */ +static double* parse_calibration_data(PyObject *pyobj, int *size) +{ + PyObject *subseq = NULL, *sstuple = NULL; + int i = 0, ncoords = 0, index = 0; + double *array = NULL, xs, ys, zs, xi, yi; + + /* check that pyobj is a sequence, and find out its size */ + if (PySequence_Check(pyobj) == 0) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a sequence of coordinate " \ + "sequences."); + return NULL; + } + ncoords = PySequence_Size(pyobj); + *size = ncoords; + + /* allocate memory */ + array = PyMem_Malloc(sizeof(double) * ncoords * 5); + if (array == NULL) + return NULL; + + /* iterate over each sub-sequence within pyobj, performing appropriate + * checks and fetching data. */ + for (i = 0; i < ncoords; i++) + { + subseq = PySequence_GetItem(pyobj, i); + if (PySequence_Check(subseq) == 0) + { + Py_DECREF(subseq); + PyMem_Free(array); + PyErr_SetString(PyExc_TypeError, + "First argument must be a sequence of " \ + "coordinate sequences."); + return NULL; + } + sstuple = PySequence_Tuple(subseq); + Py_DECREF(subseq); + if (sstuple == NULL) + { + Py_DECREF(sstuple); + PyMem_Free(array); + PyErr_SetString(PyExc_TypeError, + "First argument must be a sequence of " \ + "coordinate sequences."); + return NULL; + } + + if (!PyArg_ParseTuple(sstuple, "ddddd", &xs, &ys, &zs, + &xi, &yi)) + { + Py_DECREF(sstuple); + PyMem_Free(array); + PyErr_SetString(PyExc_TypeError, + "First argument's coordinate sequences must " \ + "contain 5 elements each: [x,y,z,xi,yi]"); + return NULL; + } + + array[index++] = xs; + array[index++] = ys; + array[index++] = zs; + array[index++] = xi; + array[index++] = yi; + + /* printf("xs = %f, ys = %f, zs = %f, xi = %f, yi = %f\n", + xs, ys, zs, xi, yi); */ + + Py_DECREF(sstuple); + } + + return array; +} + +/** + * Parses a mapping containing both camera constants and camera parameters. + * The mapping should contain mappings whose keys are all strings. The + * following keys are recognized: + * Ncx + * Nfx + * dx + * dy + * dpx + * dpy + * Cx + * Cy + * sx + * f + * kappa1 + * p1 + * p2 + * Tx + * Ty + * Tz + * Rx + * Ry + * Rz + * r1 + * r2 + * r3 + * r4 + * r5 + * r6 + * r7 + * r8 + * r9 + * + * The method returns 1 on success and 0 on failure. If the method fails, an + * appropriate exception is raised. + */ +#define TSAI_PARSE_ITEM(strx,name) { \ + mo = PyMapping_GetItemString(obj, #name); \ + if (mo != NULL) \ + { \ + number = PyNumber_Float(mo); \ + Py_DECREF(mo); \ + if (number == NULL) \ + { \ + PyErr_SetString(PyExc_TypeError, \ + "Camera parameter \"" #name \ + "\" should be a number."); \ + return 0; \ + } \ + strx.name = PyFloat_AsDouble(number); \ + Py_DECREF(number); \ + } \ +} +static int parse_camera_mapping(PyObject *obj) +{ + PyObject *mo = NULL; + PyObject *number = NULL; + + /* check that we have been passed a mapping */ + if (PyMapping_Check(obj) == 0) + { + PyErr_SetString(PyExc_TypeError, + "Second argument must be a mapping for camera " \ + "parameters."); + return 0; + } + + /* parse all known items */ + TSAI_PARSE_ITEM(tsai_cp,Ncx); + TSAI_PARSE_ITEM(tsai_cp,Nfx); + TSAI_PARSE_ITEM(tsai_cp,dx); + TSAI_PARSE_ITEM(tsai_cp,dy); + TSAI_PARSE_ITEM(tsai_cp,dpx); + TSAI_PARSE_ITEM(tsai_cp,dpy); + TSAI_PARSE_ITEM(tsai_cp,Cx); + TSAI_PARSE_ITEM(tsai_cp,Cy); + TSAI_PARSE_ITEM(tsai_cp,sx); + TSAI_PARSE_ITEM(tsai_cc,f); + TSAI_PARSE_ITEM(tsai_cc,kappa1); + TSAI_PARSE_ITEM(tsai_cc,p1); + TSAI_PARSE_ITEM(tsai_cc,p2); + TSAI_PARSE_ITEM(tsai_cc,Tx); + TSAI_PARSE_ITEM(tsai_cc,Ty); + TSAI_PARSE_ITEM(tsai_cc,Tz); + TSAI_PARSE_ITEM(tsai_cc,Rx); + TSAI_PARSE_ITEM(tsai_cc,Ry); + TSAI_PARSE_ITEM(tsai_cc,Rz); + TSAI_PARSE_ITEM(tsai_cc,r1); + TSAI_PARSE_ITEM(tsai_cc,r2); + TSAI_PARSE_ITEM(tsai_cc,r3); + TSAI_PARSE_ITEM(tsai_cc,r4); + TSAI_PARSE_ITEM(tsai_cc,r5); + TSAI_PARSE_ITEM(tsai_cc,r6); + TSAI_PARSE_ITEM(tsai_cc,r7); + TSAI_PARSE_ITEM(tsai_cc,r8); + TSAI_PARSE_ITEM(tsai_cc,r9); + + /* clear any exceptions that may have occurred while trying to fetch + * keys */ + PyErr_Clear(); + + /* return success */ + return 1; +} +#undef TSAI_PARSE_ITEM + +/** + * Constructs a mapping containing all camera parameters. For parameters that + * are known, see the parse_camera_mapping() function. + */ +static PyObject* build_camera_mapping() +{ + /* 28 parameters */ + return Py_BuildValue( + "{sdsdsdsdsdsdsdsdsdsd" \ + "sdsdsdsdsdsdsdsdsdsd" \ + "sdsdsdsdsdsdsdsd}", + "Ncx", tsai_cp.Ncx, "Nfx", tsai_cp.Nfx, + "dx", tsai_cp.dx, "dy", tsai_cp.dy, + "dpx", tsai_cp.dpx, "dpy", tsai_cp.dpy, + "Cx", tsai_cp.Cx, "Cy", tsai_cp.Cy, + "sx", tsai_cp.sx, + "f", tsai_cc.f, + "kappa1", tsai_cc.kappa1, + "p1", tsai_cc.p1, "p2", tsai_cc.p2, + "Tx", tsai_cc.Tx, "Ty", tsai_cc.Ty, "Tz", tsai_cc.Tz, + "Rx", tsai_cc.Rx, "Ry", tsai_cc.Ry, "Rz", tsai_cc.Rz, + "r1", tsai_cc.r1, "r2", tsai_cc.r2, "r3", tsai_cc.r3, + "r4", tsai_cc.r4, "r5", tsai_cc.r5, "r6", tsai_cc.r6, + "r7", tsai_cc.r7, "r8", tsai_cc.r8, "r9", tsai_cc.r9); +} + +/** + * Performs coplanar calibration with optimization of f, Tz and kappa1. + * The arguments to the function are: + * 1 - set of calibration coordinates. + * 2 - dictionary of camera parameters. + */ +static PyObject* tsai_coplanar_calibration(PyObject *self, PyObject *args) +{ + int i, index; + PyObject *calibration_data = NULL; + PyObject *params = NULL; + double *calibration_array = NULL; + int ncalibration_coords = 0; + + /* clear any error flags */ + pytsai_clear(); + + if (!PyArg_ParseTuple(args, "OO", &calibration_data, ¶ms)) + return NULL; + + /* fetch the calibration data */ + calibration_array = parse_calibration_data(calibration_data, + &ncalibration_coords); + if (calibration_array == NULL) + return NULL; + index = 0; + tsai_cd.point_count = ncalibration_coords; + for (i = 0; i < ncalibration_coords; i++) + { + tsai_cd.xw[i] = calibration_array[index++]; + tsai_cd.yw[i] = calibration_array[index++]; + tsai_cd.zw[i] = calibration_array[index++]; + tsai_cd.Xf[i] = calibration_array[index++]; + tsai_cd.Yf[i] = calibration_array[index++]; + } + PyMem_Free(calibration_array); + + /* fetch the camera parameters mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + coplanar_calibration(); + + /* check for an error */ + if (pytsai_haserror()) { + PyErr_SetString(PyExc_RuntimeError, pytsai_string); + return NULL; + } else { + return build_camera_mapping(); + } +} + + +/** + * Performs non-coplanar calibration with optimization of f, Tz and kappa1. + * The arguments to the function are: + * 1 - set of calibration coordinates. + * 2 - dictionary of camera parameters. + */ +static PyObject* tsai_noncoplanar_calibration(PyObject *self, PyObject *args) +{ + int i, index; + PyObject *calibration_data = NULL; + PyObject *params = NULL; + double *calibration_array = NULL; + int ncalibration_coords = 0; + + /* clear any error flags */ + pytsai_clear(); + + if (!PyArg_ParseTuple(args, "OO", &calibration_data, ¶ms)) + return NULL; + + /* fetch the calibration data */ + calibration_array = parse_calibration_data(calibration_data, + &ncalibration_coords); + if (calibration_array == NULL) + return NULL; + index = 0; + tsai_cd.point_count = ncalibration_coords; + for (i = 0; i < ncalibration_coords; i++) + { + tsai_cd.xw[i] = calibration_array[index++]; + tsai_cd.yw[i] = calibration_array[index++]; + tsai_cd.zw[i] = calibration_array[index++]; + tsai_cd.Xf[i] = calibration_array[index++]; + tsai_cd.Yf[i] = calibration_array[index++]; + } + PyMem_Free(calibration_array); + + /* fetch the camera parameters mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + noncoplanar_calibration(); + + /* check for an error */ + if (pytsai_haserror()) { + PyErr_SetString(PyExc_RuntimeError, pytsai_string); + return NULL; + } else { + return build_camera_mapping(); + } +} + + +/** + * Performs coplanar calibration with full optimization. + * The arguments to the function are: + * 1 - set of calibration coordinates. + * 2 - dictionary of camera parameters. + */ +static PyObject* tsai_coplanar_calibration_fo(PyObject *self, PyObject *args) +{ + int i, index; + PyObject *calibration_data = NULL; + PyObject *params = NULL; + double *calibration_array = NULL; + int ncalibration_coords = 0; + + /* clear any error flags */ + pytsai_clear(); + + if (!PyArg_ParseTuple(args, "OO", &calibration_data, ¶ms)) + return NULL; + + /* fetch the calibration data */ + calibration_array = parse_calibration_data(calibration_data, + &ncalibration_coords); + if (calibration_array == NULL) + return NULL; + index = 0; + tsai_cd.point_count = ncalibration_coords; + for (i = 0; i < ncalibration_coords; i++) + { + tsai_cd.xw[i] = calibration_array[index++]; + tsai_cd.yw[i] = calibration_array[index++]; + tsai_cd.zw[i] = calibration_array[index++]; + tsai_cd.Xf[i] = calibration_array[index++]; + tsai_cd.Yf[i] = calibration_array[index++]; + } + PyMem_Free(calibration_array); + + /* fetch the camera parameters mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + coplanar_calibration_with_full_optimization(); + + /* check for an error */ + if (pytsai_haserror()) { + PyErr_SetString(PyExc_RuntimeError, pytsai_string); + return NULL; + } else { + return build_camera_mapping(); + } +} + + +/** + * Performs non-coplanar calibration with full optimization. + * The arguments to the function are: + * 1 - set of calibration coordinates. + * 2 - dictionary of camera parameters. + */ +static PyObject* tsai_noncoplanar_calibration_fo(PyObject *self, + PyObject *args) +{ + int i, index; + PyObject *calibration_data = NULL; + PyObject *params = NULL; + double *calibration_array = NULL; + int ncalibration_coords = 0; + + /* clear any error flags */ + pytsai_clear(); + + if (!PyArg_ParseTuple(args, "OO", &calibration_data, ¶ms)) + return NULL; + + /* fetch the calibration data */ + calibration_array = parse_calibration_data(calibration_data, + &ncalibration_coords); + if (calibration_array == NULL) + return NULL; + index = 0; + tsai_cd.point_count = ncalibration_coords; + for (i = 0; i < ncalibration_coords; i++) + { + tsai_cd.xw[i] = calibration_array[index++]; + tsai_cd.yw[i] = calibration_array[index++]; + tsai_cd.zw[i] = calibration_array[index++]; + tsai_cd.Xf[i] = calibration_array[index++]; + tsai_cd.Yf[i] = calibration_array[index++]; + } + PyMem_Free(calibration_array); + + /* fetch the camera parameters mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + noncoplanar_calibration_with_full_optimization(); + + /* check for an error */ + if (pytsai_haserror()) { + PyErr_SetString(PyExc_RuntimeError, pytsai_string); + return NULL; + } else { + return build_camera_mapping(); + } +} + + +/** + * Converts from world coordinates to image coordinates. + * The arguments to the function are: + * 1 - world coordinates (3-tuple) + * 2 - dictionary of camera parameters + * It returns a 2-tuple containing (Xf, Yf). + */ +static PyObject* tsai_wc2ic(PyObject *self, PyObject *args) +{ + double xw, yw, zw, Xf, Yf; + PyObject *wc = NULL, *params = NULL, *wc2 = NULL; + + /* parse arguments */ + if (!PyArg_ParseTuple(args, "OO", &wc, ¶ms)) + return NULL; + if (!PyTuple_Check(wc)) + { + wc2 = PySequence_Tuple(wc); + if (wc2 == NULL) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + } + else + { + wc2 = wc; + Py_INCREF(wc2); + } + if (!PyArg_ParseTuple(wc2, "ddd", &xw, &yw, &zw)) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + Py_DECREF(wc2); + + /* fetch the camera parameter mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + world_coord_to_image_coord(xw, yw, zw, &Xf, &Yf); + + /* return the value (Xf, Yf) */ + return Py_BuildValue("dd", Xf, Yf); +} + + +/** + * Converts from image coordinates (+depth) to world coordinates. + * The arguments to the function are: + * 1 - image coordinates (3-tuple) (Xf, Yf, z) + * 2 - dictionary of camera parameters + * It returns a 2-tuple containing (xw, yw, zw). + */ +static PyObject* tsai_ic2wc(PyObject *self, PyObject *args) +{ + double xw, yw, zw, Xf, Yf; + PyObject *wc = NULL, *params = NULL, *wc2 = NULL; + + /* parse arguments */ + if (!PyArg_ParseTuple(args, "OO", &wc, ¶ms)) + return NULL; + if (!PyTuple_Check(wc)) + { + wc2 = PySequence_Tuple(wc); + if (wc2 == NULL) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + } + else + { + wc2 = wc; + Py_INCREF(wc2); + } + if (!PyArg_ParseTuple(wc2, "ddd", &Xf, &Yf, &zw)) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + Py_DECREF(wc2); + + /* fetch the camera parameter mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + image_coord_to_world_coord(Xf, Yf, zw, &xw, &yw); + + /* return the value (xw, yw, zw) */ + return Py_BuildValue("ddd", xw, yw, zw); +} + + +/** + * Converts from camera coordinates to world coordinates. + * The arguments to the function are: + * 1 - camera coordinates (3-tuple) (xc, yc, zc) + * 2 - dictionary of camera parameters + * It returns a 2-tuple containing (xw, yw, zw). + */ +static PyObject* tsai_cc2wc(PyObject *self, PyObject *args) +{ + double xw, yw, zw, xc, yc, zc; + PyObject *cc = NULL, *params = NULL, *cc2 = NULL; + + /* parse arguments */ + if (!PyArg_ParseTuple(args, "OO", &cc, ¶ms)) + return NULL; + if (!PyTuple_Check(cc)) + { + cc2 = PySequence_Tuple(cc); + if (cc2 == NULL) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + } + else + { + cc2 = cc; + Py_INCREF(cc2); + } + if (!PyArg_ParseTuple(cc2, "ddd", &xc, &yc, &zc)) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + Py_DECREF(cc2); + + /* fetch the camera parameter mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + camera_coord_to_world_coord(xc, yc, zc, &xw, &yw, &zw); + + /* return the value (xw, yw, zw) */ + return Py_BuildValue("ddd", xw, yw, zw); +} + + +/** + * Adds distortion to un-distorted sensor coordinates, returning distorted + * sensor coordinates. The arguments to this function are: + * 1 - Xu + * 2 - Yu + * 3 - dictionary of camera parameters + * It returns a 2-tuple containing (Xd, Yd). + */ +static PyObject* tsai_add_sensor_coord_distortion(PyObject *self, + PyObject *args) +{ + double Xu, Yu, Xd, Yd; + PyObject *params = NULL; + + /* parse arguments */ + if (!PyArg_ParseTuple(args, "ddO", &Xu, &Yu, ¶ms)) + return NULL; + + /* fetch the camera parameter mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + undistorted_to_distorted_sensor_coord(Xu, Yu, &Xd, &Yd); + + /* return the value (Xd, Yd) */ + return Py_BuildValue("dd", Xd, Yd); +} + diff --git a/others/pyTsai/src/pytsai-py3.c b/others/pyTsai/src/pytsai-py3.c new file mode 100644 index 0000000..4fe6281 --- /dev/null +++ b/others/pyTsai/src/pytsai-py3.c @@ -0,0 +1,723 @@ +/** + * pytsai.c + * Copyright (C) Jonathan Merritt 2004. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#include "Python.h" +#include "tsai/cal_main.h" +#include "errors.h" + +/************************************* + * Forward Declarations of Functions * + *************************************/ +PyMODINIT_FUNC initpytsai(void); +static double* parse_calibration_data(PyObject *pyobj, int *size); +static int parse_camera_mapping(PyObject *obj); +static PyObject* build_camera_mapping(); +static PyObject* tsai_coplanar_calibration(PyObject *self, PyObject *args); +static PyObject* tsai_noncoplanar_calibration(PyObject *self, PyObject *args); +static PyObject* tsai_coplanar_calibration_fo(PyObject *self, PyObject *args); +static PyObject* tsai_noncoplanar_calibration_fo(PyObject *self, + PyObject *args); +static PyObject* tsai_wc2ic(PyObject *self, PyObject *args); +static PyObject* tsai_ic2wc(PyObject *self, PyObject *args); +static PyObject* tsai_cc2wc(PyObject *self, PyObject *args); +static PyObject* tsai_add_sensor_coord_distortion(PyObject *self, + PyObject *args); + +/*********************** + * Module Method Table * + ***********************/ +static PyMethodDef TsaiMethods[] = +{ + + {"_pytsai_coplanar_calibration", tsai_coplanar_calibration, + METH_VARARGS, "Low level coplanar calibration routine."}, + + {"_pytsai_noncoplanar_calibration", tsai_noncoplanar_calibration, + METH_VARARGS, "Low level non-coplanar calibration routine."}, + + {"_pytsai_coplanar_calibration_fo", tsai_coplanar_calibration_fo, + METH_VARARGS, + "Low level coplanar, with full optimization calibration routine."}, + + {"_pytsai_noncoplanar_calibration_fo", tsai_noncoplanar_calibration_fo, + METH_VARARGS, + "Low level noncoplanar, with full optimization calibration routine."}, + + {"_pytsai_wc2ic", tsai_wc2ic, METH_VARARGS, + "Low level conversion of world coordinates to image coordinates."}, + + {"_pytsai_ic2wc", tsai_ic2wc, METH_VARARGS, + "Low level conversion of image coordinates to world coordinates."}, + + {"_pytsai_cc2wc", tsai_cc2wc, METH_VARARGS, + "Low level conversion of camera coordinates to world coordinates."}, + + {"_pytsai_add_sensor_coord_distortion", + tsai_add_sensor_coord_distortion, METH_VARARGS, + "Low level conversion of undistorted to distorted image coordinates."}, + + {NULL, NULL, 0, NULL} + +}; + +/*********************** + * Module Definition * + ***********************/ + +static struct PyModuleDef pytsaimodule = { + PyModuleDef_HEAD_INIT, + "pytsai", /* m_name */ + "Python Tsai method module", /* m_doc */ + -1, /* m_size */ + TsaiMethods, /* m_methods */ + NULL, /* m_reload */ + NULL, /* m_traverse */ + NULL, /* m_clear */ + NULL, /* m_free */ + }; + +/**************************** + * Function Implementations * + ****************************/ + +//PyMODINIT_FUNC initpytsai(void) +PyMODINIT_FUNC PyInit_pytsai(void) +{ + //(void) Py_InitModule("pytsai", TsaiMethods); + return PyModule_Create(&pytsaimodule); +} + +/** + * Parses calibration data. The calibration data should be in the form of a + * sequence of sequences. eg: + * [ + * [ xs, ys, zs, xi, yi ], ... + * ] + * where (xs, ys, zs) are the coordinates of a point in 3D space, and (xi, yi) + * are the coordinates of the corresponding point in an image. + * + * If the method fails, it returns NULL and raises an appropriate exception. + * On success, a flat array of doubles will be PyMem_Malloc()'d and filled + * with the calibration data: + * { xs1, ys1, zs1, xi1, yi1, xs2, ys2, zs2, xi2, yi2, ... } + * size will be populated with the number of calibration points. + */ +static double* parse_calibration_data(PyObject *pyobj, int *size) +{ + PyObject *subseq = NULL, *sstuple = NULL; + int i = 0, ncoords = 0, index = 0; + double *array = NULL, xs, ys, zs, xi, yi; + + /* check that pyobj is a sequence, and find out its size */ + if (PySequence_Check(pyobj) == 0) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a sequence of coordinate " \ + "sequences."); + return NULL; + } + ncoords = PySequence_Size(pyobj); + *size = ncoords; + + /* allocate memory */ + array = PyMem_Malloc(sizeof(double) * ncoords * 5); + if (array == NULL) + return NULL; + + /* iterate over each sub-sequence within pyobj, performing appropriate + * checks and fetching data. */ + for (i = 0; i < ncoords; i++) + { + subseq = PySequence_GetItem(pyobj, i); + if (PySequence_Check(subseq) == 0) + { + Py_DECREF(subseq); + PyMem_Free(array); + PyErr_SetString(PyExc_TypeError, + "First argument must be a sequence of " \ + "coordinate sequences."); + return NULL; + } + sstuple = PySequence_Tuple(subseq); + Py_DECREF(subseq); + if (sstuple == NULL) + { + Py_DECREF(sstuple); + PyMem_Free(array); + PyErr_SetString(PyExc_TypeError, + "First argument must be a sequence of " \ + "coordinate sequences."); + return NULL; + } + + if (!PyArg_ParseTuple(sstuple, "ddddd", &xs, &ys, &zs, + &xi, &yi)) + { + Py_DECREF(sstuple); + PyMem_Free(array); + PyErr_SetString(PyExc_TypeError, + "First argument's coordinate sequences must " \ + "contain 5 elements each: [x,y,z,xi,yi]"); + return NULL; + } + + array[index++] = xs; + array[index++] = ys; + array[index++] = zs; + array[index++] = xi; + array[index++] = yi; + + /* printf("xs = %f, ys = %f, zs = %f, xi = %f, yi = %f\n", + xs, ys, zs, xi, yi); */ + + Py_DECREF(sstuple); + } + + return array; +} + +/** + * Parses a mapping containing both camera constants and camera parameters. + * The mapping should contain mappings whose keys are all strings. The + * following keys are recognized: + * Ncx + * Nfx + * dx + * dy + * dpx + * dpy + * Cx + * Cy + * sx + * f + * kappa1 + * p1 + * p2 + * Tx + * Ty + * Tz + * Rx + * Ry + * Rz + * r1 + * r2 + * r3 + * r4 + * r5 + * r6 + * r7 + * r8 + * r9 + * + * The method returns 1 on success and 0 on failure. If the method fails, an + * appropriate exception is raised. + */ +#define TSAI_PARSE_ITEM(strx,name) { \ + mo = PyMapping_GetItemString(obj, #name); \ + if (mo != NULL) \ + { \ + number = PyNumber_Float(mo); \ + Py_DECREF(mo); \ + if (number == NULL) \ + { \ + PyErr_SetString(PyExc_TypeError, \ + "Camera parameter \"" #name \ + "\" should be a number."); \ + return 0; \ + } \ + strx.name = PyFloat_AsDouble(number); \ + Py_DECREF(number); \ + } \ +} +static int parse_camera_mapping(PyObject *obj) +{ + PyObject *mo = NULL; + PyObject *number = NULL; + + /* check that we have been passed a mapping */ + if (PyMapping_Check(obj) == 0) + { + PyErr_SetString(PyExc_TypeError, + "Second argument must be a mapping for camera " \ + "parameters."); + return 0; + } + + /* parse all known items */ + TSAI_PARSE_ITEM(tsai_cp,Ncx); + TSAI_PARSE_ITEM(tsai_cp,Nfx); + TSAI_PARSE_ITEM(tsai_cp,dx); + TSAI_PARSE_ITEM(tsai_cp,dy); + TSAI_PARSE_ITEM(tsai_cp,dpx); + TSAI_PARSE_ITEM(tsai_cp,dpy); + TSAI_PARSE_ITEM(tsai_cp,Cx); + TSAI_PARSE_ITEM(tsai_cp,Cy); + TSAI_PARSE_ITEM(tsai_cp,sx); + TSAI_PARSE_ITEM(tsai_cc,f); + TSAI_PARSE_ITEM(tsai_cc,kappa1); + TSAI_PARSE_ITEM(tsai_cc,p1); + TSAI_PARSE_ITEM(tsai_cc,p2); + TSAI_PARSE_ITEM(tsai_cc,Tx); + TSAI_PARSE_ITEM(tsai_cc,Ty); + TSAI_PARSE_ITEM(tsai_cc,Tz); + TSAI_PARSE_ITEM(tsai_cc,Rx); + TSAI_PARSE_ITEM(tsai_cc,Ry); + TSAI_PARSE_ITEM(tsai_cc,Rz); + TSAI_PARSE_ITEM(tsai_cc,r1); + TSAI_PARSE_ITEM(tsai_cc,r2); + TSAI_PARSE_ITEM(tsai_cc,r3); + TSAI_PARSE_ITEM(tsai_cc,r4); + TSAI_PARSE_ITEM(tsai_cc,r5); + TSAI_PARSE_ITEM(tsai_cc,r6); + TSAI_PARSE_ITEM(tsai_cc,r7); + TSAI_PARSE_ITEM(tsai_cc,r8); + TSAI_PARSE_ITEM(tsai_cc,r9); + + /* clear any exceptions that may have occurred while trying to fetch + * keys */ + PyErr_Clear(); + + /* return success */ + return 1; +} +#undef TSAI_PARSE_ITEM + +/** + * Constructs a mapping containing all camera parameters. For parameters that + * are known, see the parse_camera_mapping() function. + */ +static PyObject* build_camera_mapping() +{ + /* 28 parameters */ + return Py_BuildValue( + "{sdsdsdsdsdsdsdsdsdsd" \ + "sdsdsdsdsdsdsdsdsdsd" \ + "sdsdsdsdsdsdsdsd}", + "Ncx", tsai_cp.Ncx, "Nfx", tsai_cp.Nfx, + "dx", tsai_cp.dx, "dy", tsai_cp.dy, + "dpx", tsai_cp.dpx, "dpy", tsai_cp.dpy, + "Cx", tsai_cp.Cx, "Cy", tsai_cp.Cy, + "sx", tsai_cp.sx, + "f", tsai_cc.f, + "kappa1", tsai_cc.kappa1, + "p1", tsai_cc.p1, "p2", tsai_cc.p2, + "Tx", tsai_cc.Tx, "Ty", tsai_cc.Ty, "Tz", tsai_cc.Tz, + "Rx", tsai_cc.Rx, "Ry", tsai_cc.Ry, "Rz", tsai_cc.Rz, + "r1", tsai_cc.r1, "r2", tsai_cc.r2, "r3", tsai_cc.r3, + "r4", tsai_cc.r4, "r5", tsai_cc.r5, "r6", tsai_cc.r6, + "r7", tsai_cc.r7, "r8", tsai_cc.r8, "r9", tsai_cc.r9); +} + +/** + * Performs coplanar calibration with optimization of f, Tz and kappa1. + * The arguments to the function are: + * 1 - set of calibration coordinates. + * 2 - dictionary of camera parameters. + */ +static PyObject* tsai_coplanar_calibration(PyObject *self, PyObject *args) +{ + int i, index; + PyObject *calibration_data = NULL; + PyObject *params = NULL; + double *calibration_array = NULL; + int ncalibration_coords = 0; + + /* clear any error flags */ + pytsai_clear(); + + if (!PyArg_ParseTuple(args, "OO", &calibration_data, ¶ms)) + return NULL; + + /* fetch the calibration data */ + calibration_array = parse_calibration_data(calibration_data, + &ncalibration_coords); + if (calibration_array == NULL) + return NULL; + index = 0; + tsai_cd.point_count = ncalibration_coords; + for (i = 0; i < ncalibration_coords; i++) + { + tsai_cd.xw[i] = calibration_array[index++]; + tsai_cd.yw[i] = calibration_array[index++]; + tsai_cd.zw[i] = calibration_array[index++]; + tsai_cd.Xf[i] = calibration_array[index++]; + tsai_cd.Yf[i] = calibration_array[index++]; + } + PyMem_Free(calibration_array); + + /* fetch the camera parameters mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + coplanar_calibration(); + + /* check for an error */ + if (pytsai_haserror()) { + PyErr_SetString(PyExc_RuntimeError, pytsai_string); + return NULL; + } else { + return build_camera_mapping(); + } +} + + +/** + * Performs non-coplanar calibration with optimization of f, Tz and kappa1. + * The arguments to the function are: + * 1 - set of calibration coordinates. + * 2 - dictionary of camera parameters. + */ +static PyObject* tsai_noncoplanar_calibration(PyObject *self, PyObject *args) +{ + int i, index; + PyObject *calibration_data = NULL; + PyObject *params = NULL; + double *calibration_array = NULL; + int ncalibration_coords = 0; + + /* clear any error flags */ + pytsai_clear(); + + if (!PyArg_ParseTuple(args, "OO", &calibration_data, ¶ms)) + return NULL; + + /* fetch the calibration data */ + calibration_array = parse_calibration_data(calibration_data, + &ncalibration_coords); + if (calibration_array == NULL) + return NULL; + index = 0; + tsai_cd.point_count = ncalibration_coords; + for (i = 0; i < ncalibration_coords; i++) + { + tsai_cd.xw[i] = calibration_array[index++]; + tsai_cd.yw[i] = calibration_array[index++]; + tsai_cd.zw[i] = calibration_array[index++]; + tsai_cd.Xf[i] = calibration_array[index++]; + tsai_cd.Yf[i] = calibration_array[index++]; + } + PyMem_Free(calibration_array); + + /* fetch the camera parameters mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + noncoplanar_calibration(); + + /* check for an error */ + if (pytsai_haserror()) { + PyErr_SetString(PyExc_RuntimeError, pytsai_string); + return NULL; + } else { + return build_camera_mapping(); + } +} + + +/** + * Performs coplanar calibration with full optimization. + * The arguments to the function are: + * 1 - set of calibration coordinates. + * 2 - dictionary of camera parameters. + */ +static PyObject* tsai_coplanar_calibration_fo(PyObject *self, PyObject *args) +{ + int i, index; + PyObject *calibration_data = NULL; + PyObject *params = NULL; + double *calibration_array = NULL; + int ncalibration_coords = 0; + + /* clear any error flags */ + pytsai_clear(); + + if (!PyArg_ParseTuple(args, "OO", &calibration_data, ¶ms)) + return NULL; + + /* fetch the calibration data */ + calibration_array = parse_calibration_data(calibration_data, + &ncalibration_coords); + if (calibration_array == NULL) + return NULL; + index = 0; + tsai_cd.point_count = ncalibration_coords; + for (i = 0; i < ncalibration_coords; i++) + { + tsai_cd.xw[i] = calibration_array[index++]; + tsai_cd.yw[i] = calibration_array[index++]; + tsai_cd.zw[i] = calibration_array[index++]; + tsai_cd.Xf[i] = calibration_array[index++]; + tsai_cd.Yf[i] = calibration_array[index++]; + } + PyMem_Free(calibration_array); + + /* fetch the camera parameters mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + coplanar_calibration_with_full_optimization(); + + /* check for an error */ + if (pytsai_haserror()) { + PyErr_SetString(PyExc_RuntimeError, pytsai_string); + return NULL; + } else { + return build_camera_mapping(); + } +} + + +/** + * Performs non-coplanar calibration with full optimization. + * The arguments to the function are: + * 1 - set of calibration coordinates. + * 2 - dictionary of camera parameters. + */ +static PyObject* tsai_noncoplanar_calibration_fo(PyObject *self, + PyObject *args) +{ + int i, index; + PyObject *calibration_data = NULL; + PyObject *params = NULL; + double *calibration_array = NULL; + int ncalibration_coords = 0; + + /* clear any error flags */ + pytsai_clear(); + + if (!PyArg_ParseTuple(args, "OO", &calibration_data, ¶ms)) + return NULL; + + /* fetch the calibration data */ + calibration_array = parse_calibration_data(calibration_data, + &ncalibration_coords); + if (calibration_array == NULL) + return NULL; + index = 0; + tsai_cd.point_count = ncalibration_coords; + for (i = 0; i < ncalibration_coords; i++) + { + tsai_cd.xw[i] = calibration_array[index++]; + tsai_cd.yw[i] = calibration_array[index++]; + tsai_cd.zw[i] = calibration_array[index++]; + tsai_cd.Xf[i] = calibration_array[index++]; + tsai_cd.Yf[i] = calibration_array[index++]; + } + PyMem_Free(calibration_array); + + /* fetch the camera parameters mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + noncoplanar_calibration_with_full_optimization(); + + /* check for an error */ + if (pytsai_haserror()) { + PyErr_SetString(PyExc_RuntimeError, pytsai_string); + return NULL; + } else { + return build_camera_mapping(); + } +} + + +/** + * Converts from world coordinates to image coordinates. + * The arguments to the function are: + * 1 - world coordinates (3-tuple) + * 2 - dictionary of camera parameters + * It returns a 2-tuple containing (Xf, Yf). + */ +static PyObject* tsai_wc2ic(PyObject *self, PyObject *args) +{ + double xw, yw, zw, Xf, Yf; + PyObject *wc = NULL, *params = NULL, *wc2 = NULL; + + /* parse arguments */ + if (!PyArg_ParseTuple(args, "OO", &wc, ¶ms)) + return NULL; + if (!PyTuple_Check(wc)) + { + wc2 = PySequence_Tuple(wc); + if (wc2 == NULL) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + } + else + { + wc2 = wc; + Py_INCREF(wc2); + } + if (!PyArg_ParseTuple(wc2, "ddd", &xw, &yw, &zw)) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + Py_DECREF(wc2); + + /* fetch the camera parameter mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + world_coord_to_image_coord(xw, yw, zw, &Xf, &Yf); + + /* return the value (Xf, Yf) */ + return Py_BuildValue("dd", Xf, Yf); +} + + +/** + * Converts from image coordinates (+depth) to world coordinates. + * The arguments to the function are: + * 1 - image coordinates (3-tuple) (Xf, Yf, z) + * 2 - dictionary of camera parameters + * It returns a 2-tuple containing (xw, yw, zw). + */ +static PyObject* tsai_ic2wc(PyObject *self, PyObject *args) +{ + double xw, yw, zw, Xf, Yf; + PyObject *wc = NULL, *params = NULL, *wc2 = NULL; + + /* parse arguments */ + if (!PyArg_ParseTuple(args, "OO", &wc, ¶ms)) + return NULL; + if (!PyTuple_Check(wc)) + { + wc2 = PySequence_Tuple(wc); + if (wc2 == NULL) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + } + else + { + wc2 = wc; + Py_INCREF(wc2); + } + if (!PyArg_ParseTuple(wc2, "ddd", &Xf, &Yf, &zw)) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + Py_DECREF(wc2); + + /* fetch the camera parameter mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + image_coord_to_world_coord(Xf, Yf, zw, &xw, &yw); + + /* return the value (xw, yw, zw) */ + return Py_BuildValue("ddd", xw, yw, zw); +} + + +/** + * Converts from camera coordinates to world coordinates. + * The arguments to the function are: + * 1 - camera coordinates (3-tuple) (xc, yc, zc) + * 2 - dictionary of camera parameters + * It returns a 2-tuple containing (xw, yw, zw). + */ +static PyObject* tsai_cc2wc(PyObject *self, PyObject *args) +{ + double xw, yw, zw, xc, yc, zc; + PyObject *cc = NULL, *params = NULL, *cc2 = NULL; + + /* parse arguments */ + if (!PyArg_ParseTuple(args, "OO", &cc, ¶ms)) + return NULL; + if (!PyTuple_Check(cc)) + { + cc2 = PySequence_Tuple(cc); + if (cc2 == NULL) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + } + else + { + cc2 = cc; + Py_INCREF(cc2); + } + if (!PyArg_ParseTuple(cc2, "ddd", &xc, &yc, &zc)) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + Py_DECREF(cc2); + + /* fetch the camera parameter mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + camera_coord_to_world_coord(xc, yc, zc, &xw, &yw, &zw); + + /* return the value (xw, yw, zw) */ + return Py_BuildValue("ddd", xw, yw, zw); +} + + +/** + * Adds distortion to un-distorted sensor coordinates, returning distorted + * sensor coordinates. The arguments to this function are: + * 1 - Xu + * 2 - Yu + * 3 - dictionary of camera parameters + * It returns a 2-tuple containing (Xd, Yd). + */ +static PyObject* tsai_add_sensor_coord_distortion(PyObject *self, + PyObject *args) +{ + double Xu, Yu, Xd, Yd; + PyObject *params = NULL; + + /* parse arguments */ + if (!PyArg_ParseTuple(args, "ddO", &Xu, &Yu, ¶ms)) + return NULL; + + /* fetch the camera parameter mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + undistorted_to_distorted_sensor_coord(Xu, Yu, &Xd, &Yd); + + /* return the value (Xd, Yd) */ + return Py_BuildValue("dd", Xd, Yd); +} + diff --git a/others/pyTsai/src/pytsai.c b/others/pyTsai/src/pytsai.c new file mode 100644 index 0000000..4fe6281 --- /dev/null +++ b/others/pyTsai/src/pytsai.c @@ -0,0 +1,723 @@ +/** + * pytsai.c + * Copyright (C) Jonathan Merritt 2004. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#include "Python.h" +#include "tsai/cal_main.h" +#include "errors.h" + +/************************************* + * Forward Declarations of Functions * + *************************************/ +PyMODINIT_FUNC initpytsai(void); +static double* parse_calibration_data(PyObject *pyobj, int *size); +static int parse_camera_mapping(PyObject *obj); +static PyObject* build_camera_mapping(); +static PyObject* tsai_coplanar_calibration(PyObject *self, PyObject *args); +static PyObject* tsai_noncoplanar_calibration(PyObject *self, PyObject *args); +static PyObject* tsai_coplanar_calibration_fo(PyObject *self, PyObject *args); +static PyObject* tsai_noncoplanar_calibration_fo(PyObject *self, + PyObject *args); +static PyObject* tsai_wc2ic(PyObject *self, PyObject *args); +static PyObject* tsai_ic2wc(PyObject *self, PyObject *args); +static PyObject* tsai_cc2wc(PyObject *self, PyObject *args); +static PyObject* tsai_add_sensor_coord_distortion(PyObject *self, + PyObject *args); + +/*********************** + * Module Method Table * + ***********************/ +static PyMethodDef TsaiMethods[] = +{ + + {"_pytsai_coplanar_calibration", tsai_coplanar_calibration, + METH_VARARGS, "Low level coplanar calibration routine."}, + + {"_pytsai_noncoplanar_calibration", tsai_noncoplanar_calibration, + METH_VARARGS, "Low level non-coplanar calibration routine."}, + + {"_pytsai_coplanar_calibration_fo", tsai_coplanar_calibration_fo, + METH_VARARGS, + "Low level coplanar, with full optimization calibration routine."}, + + {"_pytsai_noncoplanar_calibration_fo", tsai_noncoplanar_calibration_fo, + METH_VARARGS, + "Low level noncoplanar, with full optimization calibration routine."}, + + {"_pytsai_wc2ic", tsai_wc2ic, METH_VARARGS, + "Low level conversion of world coordinates to image coordinates."}, + + {"_pytsai_ic2wc", tsai_ic2wc, METH_VARARGS, + "Low level conversion of image coordinates to world coordinates."}, + + {"_pytsai_cc2wc", tsai_cc2wc, METH_VARARGS, + "Low level conversion of camera coordinates to world coordinates."}, + + {"_pytsai_add_sensor_coord_distortion", + tsai_add_sensor_coord_distortion, METH_VARARGS, + "Low level conversion of undistorted to distorted image coordinates."}, + + {NULL, NULL, 0, NULL} + +}; + +/*********************** + * Module Definition * + ***********************/ + +static struct PyModuleDef pytsaimodule = { + PyModuleDef_HEAD_INIT, + "pytsai", /* m_name */ + "Python Tsai method module", /* m_doc */ + -1, /* m_size */ + TsaiMethods, /* m_methods */ + NULL, /* m_reload */ + NULL, /* m_traverse */ + NULL, /* m_clear */ + NULL, /* m_free */ + }; + +/**************************** + * Function Implementations * + ****************************/ + +//PyMODINIT_FUNC initpytsai(void) +PyMODINIT_FUNC PyInit_pytsai(void) +{ + //(void) Py_InitModule("pytsai", TsaiMethods); + return PyModule_Create(&pytsaimodule); +} + +/** + * Parses calibration data. The calibration data should be in the form of a + * sequence of sequences. eg: + * [ + * [ xs, ys, zs, xi, yi ], ... + * ] + * where (xs, ys, zs) are the coordinates of a point in 3D space, and (xi, yi) + * are the coordinates of the corresponding point in an image. + * + * If the method fails, it returns NULL and raises an appropriate exception. + * On success, a flat array of doubles will be PyMem_Malloc()'d and filled + * with the calibration data: + * { xs1, ys1, zs1, xi1, yi1, xs2, ys2, zs2, xi2, yi2, ... } + * size will be populated with the number of calibration points. + */ +static double* parse_calibration_data(PyObject *pyobj, int *size) +{ + PyObject *subseq = NULL, *sstuple = NULL; + int i = 0, ncoords = 0, index = 0; + double *array = NULL, xs, ys, zs, xi, yi; + + /* check that pyobj is a sequence, and find out its size */ + if (PySequence_Check(pyobj) == 0) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a sequence of coordinate " \ + "sequences."); + return NULL; + } + ncoords = PySequence_Size(pyobj); + *size = ncoords; + + /* allocate memory */ + array = PyMem_Malloc(sizeof(double) * ncoords * 5); + if (array == NULL) + return NULL; + + /* iterate over each sub-sequence within pyobj, performing appropriate + * checks and fetching data. */ + for (i = 0; i < ncoords; i++) + { + subseq = PySequence_GetItem(pyobj, i); + if (PySequence_Check(subseq) == 0) + { + Py_DECREF(subseq); + PyMem_Free(array); + PyErr_SetString(PyExc_TypeError, + "First argument must be a sequence of " \ + "coordinate sequences."); + return NULL; + } + sstuple = PySequence_Tuple(subseq); + Py_DECREF(subseq); + if (sstuple == NULL) + { + Py_DECREF(sstuple); + PyMem_Free(array); + PyErr_SetString(PyExc_TypeError, + "First argument must be a sequence of " \ + "coordinate sequences."); + return NULL; + } + + if (!PyArg_ParseTuple(sstuple, "ddddd", &xs, &ys, &zs, + &xi, &yi)) + { + Py_DECREF(sstuple); + PyMem_Free(array); + PyErr_SetString(PyExc_TypeError, + "First argument's coordinate sequences must " \ + "contain 5 elements each: [x,y,z,xi,yi]"); + return NULL; + } + + array[index++] = xs; + array[index++] = ys; + array[index++] = zs; + array[index++] = xi; + array[index++] = yi; + + /* printf("xs = %f, ys = %f, zs = %f, xi = %f, yi = %f\n", + xs, ys, zs, xi, yi); */ + + Py_DECREF(sstuple); + } + + return array; +} + +/** + * Parses a mapping containing both camera constants and camera parameters. + * The mapping should contain mappings whose keys are all strings. The + * following keys are recognized: + * Ncx + * Nfx + * dx + * dy + * dpx + * dpy + * Cx + * Cy + * sx + * f + * kappa1 + * p1 + * p2 + * Tx + * Ty + * Tz + * Rx + * Ry + * Rz + * r1 + * r2 + * r3 + * r4 + * r5 + * r6 + * r7 + * r8 + * r9 + * + * The method returns 1 on success and 0 on failure. If the method fails, an + * appropriate exception is raised. + */ +#define TSAI_PARSE_ITEM(strx,name) { \ + mo = PyMapping_GetItemString(obj, #name); \ + if (mo != NULL) \ + { \ + number = PyNumber_Float(mo); \ + Py_DECREF(mo); \ + if (number == NULL) \ + { \ + PyErr_SetString(PyExc_TypeError, \ + "Camera parameter \"" #name \ + "\" should be a number."); \ + return 0; \ + } \ + strx.name = PyFloat_AsDouble(number); \ + Py_DECREF(number); \ + } \ +} +static int parse_camera_mapping(PyObject *obj) +{ + PyObject *mo = NULL; + PyObject *number = NULL; + + /* check that we have been passed a mapping */ + if (PyMapping_Check(obj) == 0) + { + PyErr_SetString(PyExc_TypeError, + "Second argument must be a mapping for camera " \ + "parameters."); + return 0; + } + + /* parse all known items */ + TSAI_PARSE_ITEM(tsai_cp,Ncx); + TSAI_PARSE_ITEM(tsai_cp,Nfx); + TSAI_PARSE_ITEM(tsai_cp,dx); + TSAI_PARSE_ITEM(tsai_cp,dy); + TSAI_PARSE_ITEM(tsai_cp,dpx); + TSAI_PARSE_ITEM(tsai_cp,dpy); + TSAI_PARSE_ITEM(tsai_cp,Cx); + TSAI_PARSE_ITEM(tsai_cp,Cy); + TSAI_PARSE_ITEM(tsai_cp,sx); + TSAI_PARSE_ITEM(tsai_cc,f); + TSAI_PARSE_ITEM(tsai_cc,kappa1); + TSAI_PARSE_ITEM(tsai_cc,p1); + TSAI_PARSE_ITEM(tsai_cc,p2); + TSAI_PARSE_ITEM(tsai_cc,Tx); + TSAI_PARSE_ITEM(tsai_cc,Ty); + TSAI_PARSE_ITEM(tsai_cc,Tz); + TSAI_PARSE_ITEM(tsai_cc,Rx); + TSAI_PARSE_ITEM(tsai_cc,Ry); + TSAI_PARSE_ITEM(tsai_cc,Rz); + TSAI_PARSE_ITEM(tsai_cc,r1); + TSAI_PARSE_ITEM(tsai_cc,r2); + TSAI_PARSE_ITEM(tsai_cc,r3); + TSAI_PARSE_ITEM(tsai_cc,r4); + TSAI_PARSE_ITEM(tsai_cc,r5); + TSAI_PARSE_ITEM(tsai_cc,r6); + TSAI_PARSE_ITEM(tsai_cc,r7); + TSAI_PARSE_ITEM(tsai_cc,r8); + TSAI_PARSE_ITEM(tsai_cc,r9); + + /* clear any exceptions that may have occurred while trying to fetch + * keys */ + PyErr_Clear(); + + /* return success */ + return 1; +} +#undef TSAI_PARSE_ITEM + +/** + * Constructs a mapping containing all camera parameters. For parameters that + * are known, see the parse_camera_mapping() function. + */ +static PyObject* build_camera_mapping() +{ + /* 28 parameters */ + return Py_BuildValue( + "{sdsdsdsdsdsdsdsdsdsd" \ + "sdsdsdsdsdsdsdsdsdsd" \ + "sdsdsdsdsdsdsdsd}", + "Ncx", tsai_cp.Ncx, "Nfx", tsai_cp.Nfx, + "dx", tsai_cp.dx, "dy", tsai_cp.dy, + "dpx", tsai_cp.dpx, "dpy", tsai_cp.dpy, + "Cx", tsai_cp.Cx, "Cy", tsai_cp.Cy, + "sx", tsai_cp.sx, + "f", tsai_cc.f, + "kappa1", tsai_cc.kappa1, + "p1", tsai_cc.p1, "p2", tsai_cc.p2, + "Tx", tsai_cc.Tx, "Ty", tsai_cc.Ty, "Tz", tsai_cc.Tz, + "Rx", tsai_cc.Rx, "Ry", tsai_cc.Ry, "Rz", tsai_cc.Rz, + "r1", tsai_cc.r1, "r2", tsai_cc.r2, "r3", tsai_cc.r3, + "r4", tsai_cc.r4, "r5", tsai_cc.r5, "r6", tsai_cc.r6, + "r7", tsai_cc.r7, "r8", tsai_cc.r8, "r9", tsai_cc.r9); +} + +/** + * Performs coplanar calibration with optimization of f, Tz and kappa1. + * The arguments to the function are: + * 1 - set of calibration coordinates. + * 2 - dictionary of camera parameters. + */ +static PyObject* tsai_coplanar_calibration(PyObject *self, PyObject *args) +{ + int i, index; + PyObject *calibration_data = NULL; + PyObject *params = NULL; + double *calibration_array = NULL; + int ncalibration_coords = 0; + + /* clear any error flags */ + pytsai_clear(); + + if (!PyArg_ParseTuple(args, "OO", &calibration_data, ¶ms)) + return NULL; + + /* fetch the calibration data */ + calibration_array = parse_calibration_data(calibration_data, + &ncalibration_coords); + if (calibration_array == NULL) + return NULL; + index = 0; + tsai_cd.point_count = ncalibration_coords; + for (i = 0; i < ncalibration_coords; i++) + { + tsai_cd.xw[i] = calibration_array[index++]; + tsai_cd.yw[i] = calibration_array[index++]; + tsai_cd.zw[i] = calibration_array[index++]; + tsai_cd.Xf[i] = calibration_array[index++]; + tsai_cd.Yf[i] = calibration_array[index++]; + } + PyMem_Free(calibration_array); + + /* fetch the camera parameters mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + coplanar_calibration(); + + /* check for an error */ + if (pytsai_haserror()) { + PyErr_SetString(PyExc_RuntimeError, pytsai_string); + return NULL; + } else { + return build_camera_mapping(); + } +} + + +/** + * Performs non-coplanar calibration with optimization of f, Tz and kappa1. + * The arguments to the function are: + * 1 - set of calibration coordinates. + * 2 - dictionary of camera parameters. + */ +static PyObject* tsai_noncoplanar_calibration(PyObject *self, PyObject *args) +{ + int i, index; + PyObject *calibration_data = NULL; + PyObject *params = NULL; + double *calibration_array = NULL; + int ncalibration_coords = 0; + + /* clear any error flags */ + pytsai_clear(); + + if (!PyArg_ParseTuple(args, "OO", &calibration_data, ¶ms)) + return NULL; + + /* fetch the calibration data */ + calibration_array = parse_calibration_data(calibration_data, + &ncalibration_coords); + if (calibration_array == NULL) + return NULL; + index = 0; + tsai_cd.point_count = ncalibration_coords; + for (i = 0; i < ncalibration_coords; i++) + { + tsai_cd.xw[i] = calibration_array[index++]; + tsai_cd.yw[i] = calibration_array[index++]; + tsai_cd.zw[i] = calibration_array[index++]; + tsai_cd.Xf[i] = calibration_array[index++]; + tsai_cd.Yf[i] = calibration_array[index++]; + } + PyMem_Free(calibration_array); + + /* fetch the camera parameters mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + noncoplanar_calibration(); + + /* check for an error */ + if (pytsai_haserror()) { + PyErr_SetString(PyExc_RuntimeError, pytsai_string); + return NULL; + } else { + return build_camera_mapping(); + } +} + + +/** + * Performs coplanar calibration with full optimization. + * The arguments to the function are: + * 1 - set of calibration coordinates. + * 2 - dictionary of camera parameters. + */ +static PyObject* tsai_coplanar_calibration_fo(PyObject *self, PyObject *args) +{ + int i, index; + PyObject *calibration_data = NULL; + PyObject *params = NULL; + double *calibration_array = NULL; + int ncalibration_coords = 0; + + /* clear any error flags */ + pytsai_clear(); + + if (!PyArg_ParseTuple(args, "OO", &calibration_data, ¶ms)) + return NULL; + + /* fetch the calibration data */ + calibration_array = parse_calibration_data(calibration_data, + &ncalibration_coords); + if (calibration_array == NULL) + return NULL; + index = 0; + tsai_cd.point_count = ncalibration_coords; + for (i = 0; i < ncalibration_coords; i++) + { + tsai_cd.xw[i] = calibration_array[index++]; + tsai_cd.yw[i] = calibration_array[index++]; + tsai_cd.zw[i] = calibration_array[index++]; + tsai_cd.Xf[i] = calibration_array[index++]; + tsai_cd.Yf[i] = calibration_array[index++]; + } + PyMem_Free(calibration_array); + + /* fetch the camera parameters mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + coplanar_calibration_with_full_optimization(); + + /* check for an error */ + if (pytsai_haserror()) { + PyErr_SetString(PyExc_RuntimeError, pytsai_string); + return NULL; + } else { + return build_camera_mapping(); + } +} + + +/** + * Performs non-coplanar calibration with full optimization. + * The arguments to the function are: + * 1 - set of calibration coordinates. + * 2 - dictionary of camera parameters. + */ +static PyObject* tsai_noncoplanar_calibration_fo(PyObject *self, + PyObject *args) +{ + int i, index; + PyObject *calibration_data = NULL; + PyObject *params = NULL; + double *calibration_array = NULL; + int ncalibration_coords = 0; + + /* clear any error flags */ + pytsai_clear(); + + if (!PyArg_ParseTuple(args, "OO", &calibration_data, ¶ms)) + return NULL; + + /* fetch the calibration data */ + calibration_array = parse_calibration_data(calibration_data, + &ncalibration_coords); + if (calibration_array == NULL) + return NULL; + index = 0; + tsai_cd.point_count = ncalibration_coords; + for (i = 0; i < ncalibration_coords; i++) + { + tsai_cd.xw[i] = calibration_array[index++]; + tsai_cd.yw[i] = calibration_array[index++]; + tsai_cd.zw[i] = calibration_array[index++]; + tsai_cd.Xf[i] = calibration_array[index++]; + tsai_cd.Yf[i] = calibration_array[index++]; + } + PyMem_Free(calibration_array); + + /* fetch the camera parameters mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + noncoplanar_calibration_with_full_optimization(); + + /* check for an error */ + if (pytsai_haserror()) { + PyErr_SetString(PyExc_RuntimeError, pytsai_string); + return NULL; + } else { + return build_camera_mapping(); + } +} + + +/** + * Converts from world coordinates to image coordinates. + * The arguments to the function are: + * 1 - world coordinates (3-tuple) + * 2 - dictionary of camera parameters + * It returns a 2-tuple containing (Xf, Yf). + */ +static PyObject* tsai_wc2ic(PyObject *self, PyObject *args) +{ + double xw, yw, zw, Xf, Yf; + PyObject *wc = NULL, *params = NULL, *wc2 = NULL; + + /* parse arguments */ + if (!PyArg_ParseTuple(args, "OO", &wc, ¶ms)) + return NULL; + if (!PyTuple_Check(wc)) + { + wc2 = PySequence_Tuple(wc); + if (wc2 == NULL) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + } + else + { + wc2 = wc; + Py_INCREF(wc2); + } + if (!PyArg_ParseTuple(wc2, "ddd", &xw, &yw, &zw)) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + Py_DECREF(wc2); + + /* fetch the camera parameter mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + world_coord_to_image_coord(xw, yw, zw, &Xf, &Yf); + + /* return the value (Xf, Yf) */ + return Py_BuildValue("dd", Xf, Yf); +} + + +/** + * Converts from image coordinates (+depth) to world coordinates. + * The arguments to the function are: + * 1 - image coordinates (3-tuple) (Xf, Yf, z) + * 2 - dictionary of camera parameters + * It returns a 2-tuple containing (xw, yw, zw). + */ +static PyObject* tsai_ic2wc(PyObject *self, PyObject *args) +{ + double xw, yw, zw, Xf, Yf; + PyObject *wc = NULL, *params = NULL, *wc2 = NULL; + + /* parse arguments */ + if (!PyArg_ParseTuple(args, "OO", &wc, ¶ms)) + return NULL; + if (!PyTuple_Check(wc)) + { + wc2 = PySequence_Tuple(wc); + if (wc2 == NULL) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + } + else + { + wc2 = wc; + Py_INCREF(wc2); + } + if (!PyArg_ParseTuple(wc2, "ddd", &Xf, &Yf, &zw)) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + Py_DECREF(wc2); + + /* fetch the camera parameter mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + image_coord_to_world_coord(Xf, Yf, zw, &xw, &yw); + + /* return the value (xw, yw, zw) */ + return Py_BuildValue("ddd", xw, yw, zw); +} + + +/** + * Converts from camera coordinates to world coordinates. + * The arguments to the function are: + * 1 - camera coordinates (3-tuple) (xc, yc, zc) + * 2 - dictionary of camera parameters + * It returns a 2-tuple containing (xw, yw, zw). + */ +static PyObject* tsai_cc2wc(PyObject *self, PyObject *args) +{ + double xw, yw, zw, xc, yc, zc; + PyObject *cc = NULL, *params = NULL, *cc2 = NULL; + + /* parse arguments */ + if (!PyArg_ParseTuple(args, "OO", &cc, ¶ms)) + return NULL; + if (!PyTuple_Check(cc)) + { + cc2 = PySequence_Tuple(cc); + if (cc2 == NULL) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + } + else + { + cc2 = cc; + Py_INCREF(cc2); + } + if (!PyArg_ParseTuple(cc2, "ddd", &xc, &yc, &zc)) + { + PyErr_SetString(PyExc_TypeError, + "First argument must be a 3-member sequence."); + return NULL; + } + Py_DECREF(cc2); + + /* fetch the camera parameter mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + camera_coord_to_world_coord(xc, yc, zc, &xw, &yw, &zw); + + /* return the value (xw, yw, zw) */ + return Py_BuildValue("ddd", xw, yw, zw); +} + + +/** + * Adds distortion to un-distorted sensor coordinates, returning distorted + * sensor coordinates. The arguments to this function are: + * 1 - Xu + * 2 - Yu + * 3 - dictionary of camera parameters + * It returns a 2-tuple containing (Xd, Yd). + */ +static PyObject* tsai_add_sensor_coord_distortion(PyObject *self, + PyObject *args) +{ + double Xu, Yu, Xd, Yd; + PyObject *params = NULL; + + /* parse arguments */ + if (!PyArg_ParseTuple(args, "ddO", &Xu, &Yu, ¶ms)) + return NULL; + + /* fetch the camera parameter mapping */ + if (parse_camera_mapping(params) == 0) + return NULL; + + /* perform the C call */ + undistorted_to_distorted_sensor_coord(Xu, Yu, &Xd, &Yd); + + /* return the value (Xd, Yd) */ + return Py_BuildValue("dd", Xd, Yd); +} + diff --git a/others/pyTsai/src/tsai/cal_eval.c b/others/pyTsai/src/tsai/cal_eval.c new file mode 100644 index 0000000..68e9104 --- /dev/null +++ b/others/pyTsai/src/tsai/cal_eval.c @@ -0,0 +1,331 @@ +/** + * cal_eval.c + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/****************************************************************************\ +* * +* This file contains routines for measuring the accuracy of a calibrated * +* camera model. The routines are: * +* * +* distorted_image_plane_error_stats () * +* undistorted_image_plane_error_stats () * +* object_space_error_stats () * +* normalized_calibration_error () * +* * +* The routines make use of the calibrated camera parameters and calibration * +* constants contained in the two external data structures tsai_cp and tsai_cc. * +* * +* Notation * +* -------- * +* * +* The camera's X axis runs along increasing column coordinates in the * +* image/frame. The Y axis runs along increasing row coordinates. * +* All 3D coordinates are right-handed. * +* * +* History * +* ------- * +* * +* 18-May-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN * +* Split out from the cal_main.c file. * +* Simplified the statistical calculations. * +* * +\****************************************************************************/ + +#include +#include +#include "cal_main.h" + +extern struct camera_parameters tsai_cp; +extern struct calibration_constants tsai_cc; + + +/****************************************************************************\ +* This routine calculates the mean, standard deviation, max, and * +* sum-of-squared error of the magnitude of the error, in distorted image * +* coordinates, between the measured location of a feature point in the image * +* plane and the image of the 3D feature point as projected through the * +* calibrated model. The calculation is for all of the points in the * +* calibration data set. * +\****************************************************************************/ +void distorted_image_plane_error_stats (mean, stddev, max, sse) + double *mean, + *stddev, + *max, + *sse; +{ + int i; + + double Xf, + Yf, + error, + squared_error, + max_error = 0, + sum_error = 0, + sum_squared_error = 0; + + if (tsai_cd.point_count < 1) { + *mean = *stddev = *max = *sse = 0; + return; + } + + for (i = 0; i < tsai_cd.point_count; i++) { + /* calculate the ideal location of the image of the data point */ + world_coord_to_image_coord (tsai_cd.xw[i], tsai_cd.yw[i], tsai_cd.zw[i], &Xf, &Yf); + + /* determine the error between the ideal and actual location of the data point */ + /* (in distorted image coordinates) */ + squared_error = SQR (Xf - tsai_cd.Xf[i]) + SQR (Yf - tsai_cd.Yf[i]); + error = sqrt (squared_error); + sum_error += error; + sum_squared_error += squared_error; + max_error = MAX (max_error, error); + } + + *mean = sum_error / tsai_cd.point_count; + *max = max_error; + *sse = sum_squared_error; + + if (tsai_cd.point_count == 1) + *stddev = 0; + else + *stddev = sqrt ((sum_squared_error - SQR (sum_error) / tsai_cd.point_count) / (tsai_cd.point_count - 1)); +} + + +/*****************************************************************************\ +* This routine calculates the mean, standard deviation, max, and * +* sum-of-squared error of the magnitude of the error, in undistorted image * +* coordinates, between the measured location of a feature point in the image * +* plane and the image of the 3D feature point as projected through the * +* calibrated model. The calculation is for all of the points in the * +* calibration data set. * +\*****************************************************************************/ +void undistorted_image_plane_error_stats (mean, stddev, max, sse) + double *mean, + *stddev, + *max, + *sse; +{ + int i; + + double xc, + yc, + zc, + Xu_1, + Yu_1, + Xu_2, + Yu_2, + Xd, + Yd, + distortion_factor, + x_pixel_error, + y_pixel_error, + error, + squared_error, + max_error = 0, + sum_error = 0, + sum_squared_error = 0; + + if (tsai_cd.point_count < 1) { + *mean = *stddev = *max = *sse = 0; + return; + } + + for (i = 0; i < tsai_cd.point_count; i++) { + /* calculate the ideal location of the image of the data point */ + world_coord_to_camera_coord (tsai_cd.xw[i], tsai_cd.yw[i], tsai_cd.zw[i], &xc, &yc, &zc); + + /* convert from camera coordinates to undistorted sensor plane coordinates */ + Xu_1 = tsai_cc.f * xc / zc; + Yu_1 = tsai_cc.f * yc / zc; + + /* convert from 2D image coordinates to distorted sensor coordinates */ + Xd = tsai_cp.dpx * (tsai_cd.Xf[i] - tsai_cp.Cx) / tsai_cp.sx; + Yd = tsai_cp.dpy * (tsai_cd.Yf[i] - tsai_cp.Cy); + + /* convert from distorted sensor coordinates to undistorted sensor plane coordinates */ + distortion_factor = 1 + tsai_cc.kappa1 * (SQR (Xd) + SQR (Yd)); + Xu_2 = Xd * distortion_factor; + Yu_2 = Yd * distortion_factor; + + /* determine the error between the ideal and actual location of the data point */ + /* (in undistorted image coordinates) */ + x_pixel_error = tsai_cp.sx * (Xu_1 - Xu_2) / tsai_cp.dpx; + y_pixel_error = (Yu_1 - Yu_2) / tsai_cp.dpy; + squared_error = SQR (x_pixel_error) + SQR (y_pixel_error); + error = sqrt (squared_error); + sum_error += error; + sum_squared_error += squared_error; + max_error = MAX (max_error, error); + } + + *mean = sum_error / tsai_cd.point_count; + *max = max_error; + *sse = sum_squared_error; + + if (tsai_cd.point_count == 1) + *stddev = 0; + else + *stddev = sqrt ((sum_squared_error - SQR (sum_error) / tsai_cd.point_count) / (tsai_cd.point_count - 1)); +} + + +/****************************************************************************\ +* This routine calculates the mean, standard deviation, max, and * +* sum-of-squared error of the distance of closest approach (i.e. 3D error) * +* between the point in object space and the line of sight formed by back * +* projecting the measured 2D coordinates out through the camera model. * * +* The calculation is for all of the points in the calibration data set. * +\****************************************************************************/ +void object_space_error_stats (mean, stddev, max, sse) + double *mean, + *stddev, + *max, + *sse; +{ + int i; + + double xc, + yc, + zc, + Xu, + Yu, + Xd, + Yd, + t, + distortion_factor, + error, + squared_error, + max_error = 0, + sum_error = 0, + sum_squared_error = 0; + + if (tsai_cd.point_count < 1) { + *mean = *stddev = *max = *sse = 0; + return; + } + + for (i = 0; i < tsai_cd.point_count; i++) { + /* determine the position of the 3D object space point in camera coordinates */ + world_coord_to_camera_coord (tsai_cd.xw[i], tsai_cd.yw[i], tsai_cd.zw[i], &xc, &yc, &zc); + + /* convert the measured 2D image coordinates into distorted sensor coordinates */ + Xd = tsai_cp.dpx * (tsai_cd.Xf[i] - tsai_cp.Cx) / tsai_cp.sx; + Yd = tsai_cp.dpy * (tsai_cd.Yf[i] - tsai_cp.Cy); + + /* convert from distorted sensor coordinates into undistorted sensor plane coordinates */ + distortion_factor = 1 + tsai_cc.kappa1 * (SQR (Xd) + SQR (Yd)); + Xu = Xd * distortion_factor; + Yu = Yd * distortion_factor; + + /* find the magnitude of the distance (error) of closest approach */ + /* between the undistorted line of sight and the point in 3 space */ + t = (xc * Xu + yc * Yu + zc * tsai_cc.f) / (SQR (Xu) + SQR (Yu) + SQR (tsai_cc.f)); + squared_error = SQR (xc - Xu * t) + SQR (yc - Yu * t) + SQR (zc - tsai_cc.f * t); + error = sqrt (squared_error); + sum_error += error; + sum_squared_error += squared_error; + max_error = MAX (max_error, error); + } + + *mean = sum_error / tsai_cd.point_count; + *max = max_error; + *sse = sum_squared_error; + + if (tsai_cd.point_count == 1) + *stddev = 0; + else + *stddev = sqrt ((sum_squared_error - SQR (sum_error) / tsai_cd.point_count) / (tsai_cd.point_count - 1)); +} + + +/****************************************************************************\ +* This routine performs an error measure proposed by Weng in IEEE PAMI, * +* October 1992. * +\****************************************************************************/ +void normalized_calibration_error (mean, stddev) + double *mean, + *stddev; +{ + int i; + + double xc, + yc, + zc, + xc_est, + yc_est, + zc_est, + fu, + fv, + Xd, + Yd, + Xu, + Yu, + distortion_factor, + squared_error, + sum_error = 0, + sum_squared_error = 0; + + if (tsai_cd.point_count < 1) { + *mean = *stddev = 0; + return; + } + + /* This error metric is taken from */ + /* "Camera Calibration with Distortion Models and Accuracy Evaluation" */ + /* J. Weng, P. Cohen, and M. Herniou */ + /* IEEE Transactions on PAMI, Vol. 14, No. 10, October 1992, pp965-980 */ + + for (i = 0; i < tsai_cd.point_count; i++) { + /* estimate the 3D coordinates of the calibration data point by back */ + /* projecting its measured image location through the model to the */ + /* plane formed by the original z world component. */ + + /* calculate the location of the data point in camera coordinates */ + world_coord_to_camera_coord (tsai_cd.xw[i], tsai_cd.yw[i], tsai_cd.zw[i], &xc, &yc, &zc); + + /* convert from 2D image coordinates to distorted sensor coordinates */ + Xd = tsai_cp.dpx * (tsai_cd.Xf[i] - tsai_cp.Cx) / tsai_cp.sx; + Yd = tsai_cp.dpy * (tsai_cd.Yf[i] - tsai_cp.Cy); + + /* convert from distorted sensor coordinates to undistorted sensor plane coordinates */ + distortion_factor = 1 + tsai_cc.kappa1 * (SQR (Xd) + SQR (Yd)); + Xu = Xd * distortion_factor; + Yu = Yd * distortion_factor; + + /* estimate the location of the data point by back projecting the image position */ + zc_est = zc; + xc_est = zc_est * Xu / tsai_cc.f; + yc_est = zc_est * Yu / tsai_cc.f; + + fu = tsai_cp.sx * tsai_cc.f / tsai_cp.dpx; + fv = tsai_cc.f / tsai_cp.dpy; + + squared_error = (SQR (xc_est - xc) + SQR (yc_est - yc)) / + (SQR (zc_est) * (1 / SQR (fu) + 1 / SQR (fv)) / 12); + sum_error += sqrt (squared_error); + sum_squared_error += squared_error; + } + + *mean = sum_error / tsai_cd.point_count; + + if (tsai_cd.point_count == 1) + *stddev = 0; + else + *stddev = sqrt ((sum_squared_error - SQR (sum_error) / tsai_cd.point_count) / (tsai_cd.point_count - 1)); +} diff --git a/others/pyTsai/src/tsai/cal_main.c b/others/pyTsai/src/tsai/cal_main.c new file mode 100644 index 0000000..7da7fc4 --- /dev/null +++ b/others/pyTsai/src/tsai/cal_main.c @@ -0,0 +1,2630 @@ +/** + * cal_main.c + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/****************************************************************************\ +* * +* This file contains routines for calibrating Tsai's 11 parameter camera * +* model. The camera model is based on the pin hole model of 3D-2D * +* perspective projection with 1st order radial lens distortion. The model * +* consists of 5 internal (also called intrinsic or interior) camera * +* parameters: * +* * +* f - effective focal length of the pin hole camera * +* kappa1 - 1st order radial lens distortion coefficient * +* Cx, Cy - coordinates of center of radial lens distortion * +* (also used as the piercing point of the camera coordinate * +* frame's Z axis with the camera's sensor plane) * +* sx - uncertainty factor for scale of horizontal scanline * +* * +* and 6 external (also called extrinsic or exterior) camera parameters: * +* * +* Rx, Ry, Rz, Tx, Ty, Tz - rotational and translational components of * +* the transform between the world's * +* coordinate frame and the camera's * +* coordinate frame. * +* * +* Data for model calibration consists of the (x,y,z) world coordinates of a * +* feature point (in mm) and the corresponding coordinates (Xf,Yf) (in * +* pixels) of the feature point in the image. Two types of calibration are * +* available: * +* * +* coplanar - all of the calibration points lie in a single plane * +* non-coplanar - the calibration points do not lie in a single plane * +* * +* This file contains routines for two levels of calibration. The first * +* level of calibration is a direct implementation of Tsai's algorithm in * +* which only the f, Tz and kappa1 parameters are optimized for. The * +* routines are: * +* * +* coplanar_calibration () * +* noncoplanar_calibration () * +* * +* The second level of calibration optimizes for everything. This level is * +* very slow but provides the most accurate calibration. The routines are: * +* * +* coplanar_calibration_with_full_optimization () * +* noncoplanar_calibration_with_full_optimization () * +* * +* Routines are also provided for initializing camera parameter variables * +* for five of our camera/frame grabber systems. These routines are: * +* * +* initialize_photometrics_parms () * +* initialize_general_imaging_mos5300_matrox_parms () * +* initialize_panasonic_gp_mf702_matrox_parms () * +* initialize_sony_xc75_matrox_parms () * +* initialize_sony_xc77_matrox_parms () * +* initialize_sony_xc57_androx_parms () * +* * +* * +* External routines * +* ----------------- * +* * +* Nonlinear optimization for these camera calibration routines is performed * +* by the MINPACK lmdif subroutine. lmdif uses a modified * +* Levenberg-Marquardt with a jacobian calculated by a forward-difference * +* approximation. The MINPACK FORTRAN routines were translated into C * +* generated using f2c. * +* * +* Matrix operations (inversions, multiplications, etc.) are also provided by * +* external routines. * +* * +* * +* Extra notes * +* ----------- * +* * +* An explanation of the basic algorithms and description of the variables * +* can be found in several publications, including: * +* * +* "An Efficient and Accurate Camera Calibration Technique for 3D Machine * +* Vision", Roger Y. Tsai, Proceedings of IEEE Conference on Computer Vision * +* and Pattern Recognition, Miami Beach, FL, 1986, pages 364-374. * +* * +* and * +* * +* "A versatile Camera Calibration Technique for High-Accuracy 3D Machine * +* Vision Metrology Using Off-the-Shelf TV Cameras and Lenses", Roger Y. * +* Tsai, IEEE Journal of Robotics and Automation, Vol. RA-3, No. 4, August * +* 1987, pages 323-344. * +* * +* * +* Notation * +* -------- * +* * +* The camera's X axis runs along increasing column coordinates in the * +* image/frame. The Y axis runs along increasing row coordinates. * +* * +* pix == image/frame grabber picture element * +* sel == camera sensor element * +* * +* Internal routines starting with "cc_" are for coplanar calibration. * +* Internal routines starting with "ncc_" are for noncoplanar calibration. * +* * +* * +* History * +* ------- * +* * +* 01-Nov-04 Jonathan Merritt (j.merritt@pgrad.unimelb.edu.au) at * +* The University of Melbourne. * +* o Released new changes under the LGPL. * +* o Removed exit() calls. * +* * +* 20-May-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN * +* Return the error to lmdif rather than the squared error. * +* lmdif calculates the squared error internally during optimization. * +* Before this change calibration was essentially optimizing error^4. * +* Put transform and evaluation routines into separate files. * +* * +* 02-Apr-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN * +* Rewrite memory allocation to avoid memory alignment problems * +* on some machines. * +* Strip out IMSL code. MINPACK seems to work fine. * +* Filename changes for DOS port. * +* * +* 04-Jun-94 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN * +* Replaced ncc_compute_Xdp_and_Ydp with * +* ncc_compute_Xd_Yd_and_r_squared. * +* (effectively propagates the 22-Mar-94 to the non-coplanar * +* routines) * +* Added alternate macro definitions for less common math functions. * +* * +* 25-Mar-94 Torfi Thorhallsson (torfit@verk.hi.is) at the University of * +* Iceland * +* Added a new version of the routines: * +* cc_compute_exact_f_and_Tz () * +* cc_five_parm_optimization_with_late_distortion_removal () * +* cc_five_parm_optimization_with_early_distortion_removal () * +* cc_nic_optimization () * +* cc_full_optimization () * +* ncc_compute_exact_f_and_Tz () * +* ncc_nic_optimization () * +* ncc_full_optimization () * +* * +* The new routines use the *public domain* MINPACK library for * +* optimization instead of the commercial IMSL library. * +* To select the new routines, compile this file with the flag * +* -DMINPACK * +* * +* 22-Mar-94 Torfi Thorhallsson (torfit@verk.hi.is) at the University of * +* Iceland * +* Fixed a bug in cc_nic_optimization_error and * +* cc_full_optimization_error. * +* A division by cp.sx was missing. * +* * +* 15-Feb-94 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * +* Included Frederic Devernay's ( * +* * +* 04-Jul-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * +* Added new routines to evaluate the accuracy of camera calibration. * +* * +* Added check for coordinate handedness problem in calibration data. * +* * +* 01-May-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * +* For efficiency the non-linear optimizations are now all based on * +* the minimization of squared error in undistorted image coordinates * +* instead of the squared error in distorted image coordinates. * +* * +* New routine for inverse perspective projection. * +* * +* 14-Feb-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * +* Bug fixes and speed ups. * +* * +* 07-Feb-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * +* Original implementation. * +* * +\****************************************************************************/ + +#include +#include +#include +#include +#include +#include "../matrix/matrix.h" +#include "cal_main.h" +#include "../minpack/f2c.h" +#include "../minpack/minpack.h" +#include "../errors.h" + + +/* Variables used by the subroutines for I/O (perhaps not the best way of + * doing this) */ +struct camera_parameters tsai_cp; +struct calibration_data tsai_cd; +struct calibration_constants tsai_cc; + +/* Local working storage */ +static +double Xd[MAX_POINTS], + Yd[MAX_POINTS], + r_squared[MAX_POINTS], + U[7]; + + +/* char camera_type[256] = "unknown"; */ + + +/** We don't need the initialization routines in the Python version; these + * are provided by the high-level Python wrapper. */ +#if 0 + +/************************************************************************/ +void initialize_photometrics_parms () +{ + strcpy (camera_type, "Photometrics Star I"); + + tsai_cp.Ncx = 576; /* [sel] */ + tsai_cp.Nfx = 576; /* [pix] */ + tsai_cp.dx = 0.023; /* [mm/sel] */ + tsai_cp.dy = 0.023; /* [mm/sel] */ + tsai_cp.dpx = tsai_cp.dx * tsai_cp.Ncx / tsai_cp.Nfx; /* [mm/pix] */ + tsai_cp.dpy = tsai_cp.dy; /* [mm/pix] */ + tsai_cp.Cx = 576 / 2; /* [pix] */ + tsai_cp.Cy = 384 / 2; /* [pix] */ + tsai_cp.sx = 1.0; /* [] */ + + /* something a bit more realistic */ + tsai_cp.Cx = 258.0; + tsai_cp.Cy = 204.0; +} + + +/************************************************************************/ +void initialize_general_imaging_mos5300_matrox_parms () +{ + strcpy (camera_type, "General Imaging MOS5300 + Matrox"); + + tsai_cp.Ncx = 649; /* [sel] */ + tsai_cp.Nfx = 512; /* [pix] */ + tsai_cp.dx = 0.015; /* [mm/sel] */ + tsai_cp.dy = 0.015; /* [mm/sel] */ + tsai_cp.dpx = tsai_cp.dx * tsai_cp.Ncx / tsai_cp.Nfx; /* [mm/pix] */ + tsai_cp.dpy = tsai_cp.dy; /* [mm/pix] */ + tsai_cp.Cx = 512 / 2; /* [pix] */ + tsai_cp.Cy = 480 / 2; /* [pix] */ + tsai_cp.sx = 1.0; /* [] */ +} + + +/************************************************************************/ +void initialize_panasonic_gp_mf702_matrox_parms () +{ + strcpy (camera_type, "Panasonic GP-MF702 + Matrox"); + + tsai_cp.Ncx = 649; /* [sel] */ + tsai_cp.Nfx = 512; /* [pix] */ + tsai_cp.dx = 0.015; /* [mm/sel] */ + tsai_cp.dy = 0.015; /* [mm/sel] */ + tsai_cp.dpx = tsai_cp.dx * tsai_cp.Ncx / tsai_cp.Nfx; /* [mm/pix] */ + tsai_cp.dpy = tsai_cp.dy; /* [mm/pix] */ + + tsai_cp.Cx = 268; /* [pix] */ + tsai_cp.Cy = 248; /* [pix] */ + + tsai_cp.sx = 1.078647; /* [] */ +} + + +/************************************************************************/ +void initialize_sony_xc75_matrox_parms () +{ + strcpy (camera_type, "Sony XC75 + Matrox"); + + tsai_cp.Ncx = 768; /* [sel] */ + tsai_cp.Nfx = 512; /* [pix] */ + tsai_cp.dx = 0.0084; /* [mm/sel] */ + tsai_cp.dy = 0.0098; /* [mm/sel] */ + tsai_cp.dpx = tsai_cp.dx * tsai_cp.Ncx / tsai_cp.Nfx; /* [mm/pix] */ + tsai_cp.dpy = tsai_cp.dy; /* [mm/pix] */ + tsai_cp.Cx = 512 / 2; /* [pix] */ + tsai_cp.Cy = 480 / 2; /* [pix] */ + tsai_cp.sx = 1.0; /* [] */ +} + + +/************************************************************************/ +void initialize_sony_xc77_matrox_parms () +{ + strcpy (camera_type, "Sony XC77 + Matrox"); + + tsai_cp.Ncx = 768; /* [sel] */ + tsai_cp.Nfx = 512; /* [pix] */ + tsai_cp.dx = 0.011; /* [mm/sel] */ + tsai_cp.dy = 0.013; /* [mm/sel] */ + tsai_cp.dpx = tsai_cp.dx * tsai_cp.Ncx / tsai_cp.Nfx; /* [mm/pix] */ + tsai_cp.dpy = tsai_cp.dy; /* [mm/pix] */ + tsai_cp.Cx = 512 / 2; /* [pix] */ + tsai_cp.Cy = 480 / 2; /* [pix] */ + tsai_cp.sx = 1.0; /* [] */ +} + + +/************************************************************************/ +void initialize_sony_xc57_androx_parms () +{ + strcpy (camera_type, "Sony XC57 + Androx"); + + tsai_cp.Ncx = 510; /* [sel] */ + tsai_cp.Nfx = 512; /* [pix] */ + tsai_cp.dx = 0.017; /* [mm/sel] */ + tsai_cp.dy = 0.013; /* [mm/sel] */ + tsai_cp.dpx = tsai_cp.dx * tsai_cp.Ncx / tsai_cp.Nfx; /* [mm/pix] */ + tsai_cp.dpy = tsai_cp.dy; /* [mm/pix] */ + tsai_cp.Cx = 512 / 2; /* [pix] */ + tsai_cp.Cy = 480 / 2; /* [pix] */ + tsai_cp.sx = 1.107914; /* [] */ +} + + +/************************************************************************/ +/* John Krumm, 9 November 1993 */ +/* This assumes that every other row, starting with the second row from */ +/* the top, has been removed. The Xap Shot CCD only has 250 lines, and */ +/* it inserts new rows in between the real rows to make a full size */ +/* picture. Its algorithm for making these new rows isn't very good, */ +/* so it's best just to throw them away. */ +/* The camera's specs give the CCD size as 6.4(V)x4.8(H) mm. */ +/* A call to the manufacturer found that the CCD has 250 rows */ +/* and 782 columns. It is underscanned to 242 rows and 739 columns. */ +/************************************************************************/ +void initialize_xapshot_matrox_parms () +{ + strcpy (camera_type, "Canon Xap Shot"); + + tsai_cp.Ncx = 739; /* [sel] */ + tsai_cp.Nfx = 512; /* [pix] */ + tsai_cp.dx = 6.4 / 782.0; /* [mm/sel] */ + tsai_cp.dy = 4.8 / 250.0; /* [mm/sel] */ + tsai_cp.dpx = tsai_cp.dx * tsai_cp.Ncx / tsai_cp.Nfx; /* [mm/pix] */ + tsai_cp.dpy = tsai_cp.dy; /* [mm/pix] */ + tsai_cp.Cx = 512 / 2; /* [pix] */ + tsai_cp.Cy = 240 / 2; /* [pix] */ + tsai_cp.sx = 1.027753; /* from noncoplanar calibration *//* [] */ +} + +#endif /* finish excluding camera settings routines */ + + +/***********************************************************************\ +* This routine solves for the roll, pitch and yaw angles (in radians) * +* for a given orthonormal rotation matrix (from Richard P. Paul, * +* Robot Manipulators: Mathematics, Programming and Control, p70). * +* Note 1, should the rotation matrix not be orthonormal these will not * +* be the "best fit" roll, pitch and yaw angles. * +* Note 2, there are actually two possible solutions for the matrix. * +* The second solution can be found by adding 180 degrees to Rz before * +* Ry and Rx are calculated. * +\***********************************************************************/ +/* pytsai: cannot fail; void return type is fine. */ +void solve_RPY_transform (TSAI_ARGS_DECL) +{ + double sg, + cg; + + tsai_cc.Rz = atan2 (tsai_cc.r4, tsai_cc.r1); + SINCOS (tsai_cc.Rz, sg, cg); + tsai_cc.Ry = atan2 (-tsai_cc.r7, tsai_cc.r1 * cg + tsai_cc.r4 * sg); + tsai_cc.Rx = atan2 (tsai_cc.r3 * sg - tsai_cc.r6 * cg, tsai_cc.r5 * cg - tsai_cc.r2 * sg); +} + + +/***********************************************************************\ +* This routine simply takes the roll, pitch and yaw angles and fills in * +* the rotation matrix elements r1-r9. * +\***********************************************************************/ +/* pytsai: cannot fail; void return type is fine. */ +void apply_RPY_transform (TSAI_ARGS_DECL) +{ + double sa, + ca, + sb, + cb, + sg, + cg; + + SINCOS (tsai_cc.Rx, sa, ca); + SINCOS (tsai_cc.Ry, sb, cb); + SINCOS (tsai_cc.Rz, sg, cg); + + tsai_cc.r1 = cb * cg; + tsai_cc.r2 = cg * sa * sb - ca * sg; + tsai_cc.r3 = sa * sg + ca * cg * sb; + tsai_cc.r4 = cb * sg; + tsai_cc.r5 = sa * sb * sg + ca * cg; + tsai_cc.r6 = ca * sb * sg - cg * sa; + tsai_cc.r7 = -sb; + tsai_cc.r8 = cb * sa; + tsai_cc.r9 = ca * cb; +} + + +/***********************************************************************\ +* Routines for coplanar camera calibration * +\***********************************************************************/ +/* pytsai: cannot fail; void return type is fine. */ +void cc_compute_Xd_Yd_and_r_squared () +{ + int i; + + double Xd_, + Yd_; + + for (i = 0; i < tsai_cd.point_count; i++) { + Xd[i] = Xd_ = tsai_cp.dpx * (tsai_cd.Xf[i] - tsai_cp.Cx) / tsai_cp.sx; /* [mm] */ + Yd[i] = Yd_ = tsai_cp.dpy * (tsai_cd.Yf[i] - tsai_cp.Cy); /* [mm] */ + r_squared[i] = SQR (Xd_) + SQR (Yd_); /* [mm^2] */ + } +} + + +/* pytsai: can fail: need int return type. */ +int cc_compute_U () +{ + int i; + + dmat M, + a, + b; + + M = newdmat (0, (tsai_cd.point_count - 1), 0, 4, &errno); + if (errno) { + pytsai_raise ("cc compute U: unable to allocate matrix M"); + return 0; + } + + a = newdmat (0, 4, 0, 0, &errno); + if (errno) { + freemat(M); + pytsai_raise ("cc compute U: unable to allocate vector a"); + return 0; + } + + b = newdmat (0, (tsai_cd.point_count - 1), 0, 0, &errno); + if (errno) { + freemat(M); + freemat(a); + pytsai_raise ("cc compute U: unable to allocate vector b"); + return 0; + } + + for (i = 0; i < tsai_cd.point_count; i++) { + M.el[i][0] = Yd[i] * tsai_cd.xw[i]; + M.el[i][1] = Yd[i] * tsai_cd.yw[i]; + M.el[i][2] = Yd[i]; + M.el[i][3] = -Xd[i] * tsai_cd.xw[i]; + M.el[i][4] = -Xd[i] * tsai_cd.yw[i]; + b.el[i][0] = Xd[i]; + } + + if (solve_system (M, a, b)) { + freemat(M); + freemat(a); + freemat(b); + pytsai_raise ("cc compute U: unable to solve system Ma=b"); + return 0; + } + + U[0] = a.el[0][0]; + U[1] = a.el[1][0]; + U[2] = a.el[2][0]; + U[3] = a.el[3][0]; + U[4] = a.el[4][0]; + + freemat (M); + freemat (a); + freemat (b); + + return 1; +} + + +/* pytsai: cannot fail; void return type is fine. */ +void cc_compute_Tx_and_Ty () +{ + int i, + far_point; + + double Tx, + Ty, + Ty_squared, + x, + y, + Sr, + r1p, + r2p, + r4p, + r5p, + r1, + r2, + r4, + r5, + distance, + far_distance; + + r1p = U[0]; + r2p = U[1]; + r4p = U[3]; + r5p = U[4]; + + /* first find the square of the magnitude of Ty */ + if ((fabs (r1p) < EPSILON) && (fabs (r2p) < EPSILON)) + Ty_squared = 1 / (SQR (r4p) + SQR (r5p)); + else if ((fabs (r4p) < EPSILON) && (fabs (r5p) < EPSILON)) + Ty_squared = 1 / (SQR (r1p) + SQR (r2p)); + else if ((fabs (r1p) < EPSILON) && (fabs (r4p) < EPSILON)) + Ty_squared = 1 / (SQR (r2p) + SQR (r5p)); + else if ((fabs (r2p) < EPSILON) && (fabs (r5p) < EPSILON)) + Ty_squared = 1 / (SQR (r1p) + SQR (r4p)); + else { + Sr = SQR (r1p) + SQR (r2p) + SQR (r4p) + SQR (r5p); + Ty_squared = (Sr - sqrt (SQR (Sr) - 4 * SQR (r1p * r5p - r4p * r2p))) / + (2 * SQR (r1p * r5p - r4p * r2p)); + } + + /* find a point that is far from the image center */ + far_distance = 0; + far_point = 0; + for (i = 0; i < tsai_cd.point_count; i++) + if ((distance = r_squared[i]) > far_distance) { + far_point = i; + far_distance = distance; + } + + /* now find the sign for Ty */ + /* start by assuming Ty > 0 */ + Ty = sqrt (Ty_squared); + r1 = U[0] * Ty; + r2 = U[1] * Ty; + Tx = U[2] * Ty; + r4 = U[3] * Ty; + r5 = U[4] * Ty; + x = r1 * tsai_cd.xw[far_point] + r2 * tsai_cd.yw[far_point] + Tx; + y = r4 * tsai_cd.xw[far_point] + r5 * tsai_cd.yw[far_point] + Ty; + + /* flip Ty if we guessed wrong */ + if ((SIGNBIT (x) != SIGNBIT (Xd[far_point])) || + (SIGNBIT (y) != SIGNBIT (Yd[far_point]))) + Ty = -Ty; + + /* update the calibration constants */ + tsai_cc.Tx = U[2] * Ty; + tsai_cc.Ty = Ty; +} + + +/* pytsai: cannot fail; void return type is fine. */ +void cc_compute_R () +{ + double r1, + r2, + r3, + r4, + r5, + r6, + r7, + r8, + r9; + + r1 = U[0] * tsai_cc.Ty; + r2 = U[1] * tsai_cc.Ty; + r3 = sqrt (1 - SQR (r1) - SQR (r2)); + + r4 = U[3] * tsai_cc.Ty; + r5 = U[4] * tsai_cc.Ty; + r6 = sqrt (1 - SQR (r4) - SQR (r5)); + if (!SIGNBIT (r1 * r4 + r2 * r5)) + r6 = -r6; + + /* use the outer product of the first two rows to get the last row */ + r7 = r2 * r6 - r3 * r5; + r8 = r3 * r4 - r1 * r6; + r9 = r1 * r5 - r2 * r4; + + /* update the calibration constants */ + tsai_cc.r1 = r1; + tsai_cc.r2 = r2; + tsai_cc.r3 = r3; + tsai_cc.r4 = r4; + tsai_cc.r5 = r5; + tsai_cc.r6 = r6; + tsai_cc.r7 = r7; + tsai_cc.r8 = r8; + tsai_cc.r9 = r9; + + /* fill in tsai_cc.Rx, tsai_cc.Ry and tsai_cc.Rz */ + solve_RPY_transform (); +} + + +/* pytsai: can fail; need int return type */ +int cc_compute_approximate_f_and_Tz () +{ + int i; + + dmat M, + a, + b; + + M = newdmat (0, (tsai_cd.point_count - 1), 0, 1, &errno); + if (errno) { + pytsai_raise ("cc compute apx: unable to allocate matrix M"); + return 0; + } + + a = newdmat (0, 1, 0, 0, &errno); + if (errno) { + freemat(M); + pytsai_raise ("cc compute apx: unable to allocate vector a"); + return 0; + } + + b = newdmat (0, (tsai_cd.point_count - 1), 0, 0, &errno); + if (errno) { + freemat(M); + freemat(a); + pytsai_raise ("cc compute apx: unable to allocate vector b"); + return 0; + } + + for (i = 0; i < tsai_cd.point_count; i++) { + M.el[i][0] = tsai_cc.r4 * tsai_cd.xw[i] + tsai_cc.r5 * tsai_cd.yw[i] + tsai_cc.Ty; + M.el[i][1] = -Yd[i]; + b.el[i][0] = (tsai_cc.r7 * tsai_cd.xw[i] + tsai_cc.r8 * tsai_cd.yw[i]) * Yd[i]; + } + + if (solve_system (M, a, b)) { + freemat(M); + freemat(a); + freemat(b); + pytsai_raise ("cc compute apx: unable to solve system Ma=b"); + return 0; + } + + /* update the calibration constants */ + tsai_cc.f = a.el[0][0]; + tsai_cc.Tz = a.el[1][0]; + tsai_cc.kappa1 = 0.0; /* this is the assumption that our calculation was + * made under */ + + freemat (M); + freemat (a); + freemat (b); + + return 1; +} + + +/************************************************************************/ +/* pytsai: cannot fail; void return type is fine. */ +void cc_compute_exact_f_and_Tz_error ( + integer *m_ptr, /* pointer to number of points to fit */ + integer *n_ptr, /* pointer to number of parameters */ + doublereal *params, /* vector of parameters */ + doublereal *err /* vector of error from data */ +) +{ + int i; + + double f, + Tz, + kappa1, + xc, + yc, + zc, + Xu_1, + Yu_1, + Xu_2, + Yu_2, + distortion_factor; + + f = params[0]; + Tz = params[1]; + kappa1 = params[2]; + + for (i = 0; i < tsai_cd.point_count; i++) { + /* convert from world coordinates to camera coordinates */ + /* Note: zw is implicitly assumed to be zero for these (coplanar) + * calculations */ + xc = tsai_cc.r1 * tsai_cd.xw[i] + tsai_cc.r2 * tsai_cd.yw[i] + tsai_cc.Tx; + yc = tsai_cc.r4 * tsai_cd.xw[i] + tsai_cc.r5 * tsai_cd.yw[i] + tsai_cc.Ty; + zc = tsai_cc.r7 * tsai_cd.xw[i] + tsai_cc.r8 * tsai_cd.yw[i] + Tz; + + /* convert from camera coordinates to undistorted sensor coordinates */ + Xu_1 = f * xc / zc; + Yu_1 = f * yc / zc; + + /* convert from distorted sensor coordinates to undistorted sensor + * coordinates */ + distortion_factor = 1 + kappa1 * (SQR (Xd[i]) + SQR (Yd[i])); + Xu_2 = Xd[i] * distortion_factor; + Yu_2 = Yd[i] * distortion_factor; + + /* record the error in the undistorted sensor coordinates */ + err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2); + } +} + + +/* pytsai: can fail; need int return type */ +int cc_compute_exact_f_and_Tz () +{ +#define NPARAMS 3 + + int i; + + /* Parameters needed by MINPACK's lmdif() */ + + integer m = tsai_cd.point_count; + integer n = NPARAMS; + doublereal x[NPARAMS]; + doublereal *fvec; + doublereal ftol = REL_SENSOR_TOLERANCE_ftol; + doublereal xtol = REL_PARAM_TOLERANCE_xtol; + doublereal gtol = ORTHO_TOLERANCE_gtol; + integer maxfev = MAXFEV; + doublereal epsfcn = EPSFCN; + doublereal diag[NPARAMS]; + integer mode = MODE; + doublereal factor = FACTOR; + integer nprint = 0; + integer info; + integer nfev; + doublereal *fjac; + integer ldfjac = m; + integer ipvt[NPARAMS]; + doublereal qtf[NPARAMS]; + doublereal wa1[NPARAMS]; + doublereal wa2[NPARAMS]; + doublereal wa3[NPARAMS]; + doublereal *wa4; + + /* allocate some workspace */ + if (( fvec = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + pytsai_raise("malloc: Cannot allocate workspace fvec"); + return 0; + } + + if (( fjac = (doublereal *) malloc ((unsigned int) m*n * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + pytsai_raise("malloc: Cannot allocate workspace fjac"); + return 0; + } + + if (( wa4 = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + free(fjac); + pytsai_raise("malloc: Cannot allocate workspace wa4"); + return 0; + } + + /* use the current calibration constants as an initial guess */ + x[0] = tsai_cc.f; + x[1] = tsai_cc.Tz; + x[2] = tsai_cc.kappa1; + + /* define optional scale factors for the parameters */ + if ( mode == 2 ) { + for (i = 0; i < NPARAMS; i++) + diag[i] = 1.0; /* some user-defined values */ + } + + /* perform the optimization */ + lmdif_ (cc_compute_exact_f_and_Tz_error, + &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn, + diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac, + ipvt, qtf, wa1, wa2, wa3, wa4); + + /* update the calibration constants */ + tsai_cc.f = x[0]; + tsai_cc.Tz = x[1]; + tsai_cc.kappa1 = x[2]; + + /* release allocated workspace */ + free(fvec); + free(fjac); + free(wa4); + +#ifdef DEBUG + /* print the number of function calls during iteration */ + fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev); +#endif + + return 1; + +#undef NPARAMS +} + + +/************************************************************************/ +/* pytsai: can fail; int return type is required. */ +int cc_three_parm_optimization () +{ + int i; + + for (i = 0; i < tsai_cd.point_count; i++) + if (tsai_cd.zw[i]) { + pytsai_raise("error - coplanar calibration tried with data outside of Z plane"); + return 0; + } + + cc_compute_Xd_Yd_and_r_squared (); + + if (!cc_compute_U()) + return 0; + + cc_compute_Tx_and_Ty (); + + cc_compute_R (); + + if (!cc_compute_approximate_f_and_Tz()) + return 0; + + if (tsai_cc.f < 0) { + /* try the other solution for the orthonormal matrix */ + tsai_cc.r3 = -tsai_cc.r3; + tsai_cc.r6 = -tsai_cc.r6; + tsai_cc.r7 = -tsai_cc.r7; + tsai_cc.r8 = -tsai_cc.r8; + solve_RPY_transform (); + + if (!cc_compute_approximate_f_and_Tz()) + return 0; + + if (tsai_cc.f < 0) { + pytsai_raise("error - possible handedness problem with data"); + return 0; + } + } + + if (!cc_compute_exact_f_and_Tz()) + return 0; + + return 1; +} + + +/************************************************************************/ +/* pytsai: cannot fail; void return type is fine. */ +void cc_remove_sensor_plane_distortion_from_Xd_and_Yd () +{ + int i; + double Xu, + Yu; + + for (i = 0; i < tsai_cd.point_count; i++) { + distorted_to_undistorted_sensor_coord (Xd[i], Yd[i], &Xu, &Yu); + Xd[i] = Xu; + Yd[i] = Yu; + r_squared[i] = SQR (Xu) + SQR (Yu); + } +} + + +/************************************************************************/ +/* pytsai: can fail. This method is called from within the lmdif_ routine. + * To indicate failure, set *iflag = -1. */ +void cc_five_parm_optimization_with_late_distortion_removal_error (m_ptr, n_ptr, params, err, iflag) + integer *m_ptr; /* pointer to number of points to fit */ + integer *n_ptr; /* pointer to number of parameters */ + doublereal *params; /* vector of parameters */ + doublereal *err; /* vector of error from data */ + integer *iflag; /* flag to indicate error to caller */ +{ + int i; + + double f, + Tz, + kappa1, + xc, + yc, + zc, + Xu_1, + Yu_1, + Xu_2, + Yu_2, + distortion_factor; + + /* in this routine radial lens distortion is only taken into account */ + /* after the rotation and translation constants have been determined */ + + f = params[0]; + Tz = params[1]; + kappa1 = params[2]; + + tsai_cp.Cx = params[3]; + tsai_cp.Cy = params[4]; + + cc_compute_Xd_Yd_and_r_squared (); + + if (!cc_compute_U()) + { + *iflag = -1; + return; + } + + cc_compute_Tx_and_Ty (); + + cc_compute_R (); + + if (!cc_compute_approximate_f_and_Tz()) + { + *iflag = -1; + return; + } + + if (tsai_cc.f < 0) { + /* try the other solution for the orthonormal matrix */ + tsai_cc.r3 = -tsai_cc.r3; + tsai_cc.r6 = -tsai_cc.r6; + tsai_cc.r7 = -tsai_cc.r7; + tsai_cc.r8 = -tsai_cc.r8; + solve_RPY_transform (); + + if (!cc_compute_approximate_f_and_Tz()) + { + *iflag = -1; + return; + } + + if (tsai_cc.f < 0) { + pytsai_raise("error - possible handedness problem with data"); + *iflag = -1; + return; + } + } + + for (i = 0; i < tsai_cd.point_count; i++) { + /* convert from world coordinates to camera coordinates */ + /* Note: zw is implicitly assumed to be zero for these (coplanar) calculations */ + xc = tsai_cc.r1 * tsai_cd.xw[i] + tsai_cc.r2 * tsai_cd.yw[i] + tsai_cc.Tx; + yc = tsai_cc.r4 * tsai_cd.xw[i] + tsai_cc.r5 * tsai_cd.yw[i] + tsai_cc.Ty; + zc = tsai_cc.r7 * tsai_cd.xw[i] + tsai_cc.r8 * tsai_cd.yw[i] + Tz; + + /* convert from camera coordinates to undistorted sensor coordinates */ + Xu_1 = f * xc / zc; + Yu_1 = f * yc / zc; + + /* convert from distorted sensor coordinates to undistorted sensor coordinates */ + distortion_factor = 1 + kappa1 * r_squared[i]; + Xu_2 = Xd[i] * distortion_factor; + Yu_2 = Yd[i] * distortion_factor; + + /* record the error in the undistorted sensor coordinates */ + err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2); + } + +} + + +/* pytsai: can fail; need int return type. */ +int cc_five_parm_optimization_with_late_distortion_removal () +{ +#define NPARAMS 5 + + int i; + + /* Parameters needed by MINPACK's lmdif() */ + + integer m = tsai_cd.point_count; + integer n = NPARAMS; + doublereal x[NPARAMS]; + doublereal *fvec; + doublereal ftol = REL_SENSOR_TOLERANCE_ftol; + doublereal xtol = REL_PARAM_TOLERANCE_xtol; + doublereal gtol = ORTHO_TOLERANCE_gtol; + integer maxfev = MAXFEV; + doublereal epsfcn = EPSFCN; + doublereal diag[NPARAMS]; + integer mode = MODE; + doublereal factor = FACTOR; + integer nprint = 0; + integer info; + integer nfev; + doublereal *fjac; + integer ldfjac = m; + integer ipvt[NPARAMS]; + doublereal qtf[NPARAMS]; + doublereal wa1[NPARAMS]; + doublereal wa2[NPARAMS]; + doublereal wa3[NPARAMS]; + doublereal *wa4; + + /* allocate some workspace */ + if (( fvec = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + pytsai_raise("malloc: Cannot allocate workspace fvec"); + return 0; + } + + if (( fjac = (doublereal *) malloc ((unsigned int) m*n * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + pytsai_raise("malloc: Cannot allocate workspace fjac"); + return 0; + } + + if (( wa4 = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + free(fjac); + pytsai_raise("malloc: Cannot allocate workspace wa4"); + return 0; + } + + /* use the current calibration constants as an initial guess */ + x[0] = tsai_cc.f; + x[1] = tsai_cc.Tz; + x[2] = tsai_cc.kappa1; + x[3] = tsai_cp.Cx; + x[4] = tsai_cp.Cy; + + /* define optional scale factors for the parameters */ + if ( mode == 2 ) { + for (i = 0; i < NPARAMS; i++) + diag[i] = 1.0; /* some user-defined values */ + } + + /* perform the optimization */ + lmdif_ (cc_five_parm_optimization_with_late_distortion_removal_error, + &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn, + diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac, + ipvt, qtf, wa1, wa2, wa3, wa4); + /* check for pytsai error condition */ + if (info == -1) + { + free(fvec); + free(fjac); + free(wa4); + return 0; + } + /* TODO: Check for and translate other error conditions. See + * lmdif.c for possible values of the info parameter. */ + + /* update the calibration and camera constants */ + tsai_cc.f = x[0]; + tsai_cc.Tz = x[1]; + tsai_cc.kappa1 = x[2]; + tsai_cp.Cx = x[3]; + tsai_cp.Cy = x[4]; + + /* release allocated workspace */ + free(fvec); + free(fjac); + free(wa4); + +#ifdef DEBUG + /* print the number of function calls during iteration */ + fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev); +#endif + + return 1; + +#undef NPARAMS +} + + +/************************************************************************/ +/* pytsai: can fail. This method is called from within the lmdif_ routine. + * To indicate failure, set *iflag = -1. */ +void cc_five_parm_optimization_with_early_distortion_removal_error (m_ptr, n_ptr, params, err, iflag) + integer *m_ptr; /* pointer to number of points to fit */ + integer *n_ptr; /* pointer to number of parameters */ + doublereal *params; /* vector of parameters */ + doublereal *err; /* vector of error from data */ + integer *iflag; /* flag to indicate error to caller */ +{ + int i; + + double f, + Tz, + xc, + yc, + zc, + Xu_1, + Yu_1, + Xu_2, + Yu_2; + + /* in this routine radial lens distortion is taken into account */ + /* before the rotation and translation constants are determined */ + /* (this assumes we have the distortion reasonably modelled) */ + + f = params[0]; + Tz = params[1]; + + tsai_cc.kappa1 = params[2]; + tsai_cp.Cx = params[3]; + tsai_cp.Cy = params[4]; + + cc_compute_Xd_Yd_and_r_squared (); + + /* remove the sensor distortion before computing the translation and rotation stuff */ + cc_remove_sensor_plane_distortion_from_Xd_and_Yd (); + + if (!cc_compute_U()) + { + *iflag = -1; + return; + } + + cc_compute_Tx_and_Ty (); + + cc_compute_R (); + + /* we need to do this just to see if we have to flip the rotation matrix */ + if (!cc_compute_approximate_f_and_Tz()) + { + *iflag = -1; + return; + } + + if (tsai_cc.f < 0) { + /* try the other solution for the orthonormal matrix */ + tsai_cc.r3 = -tsai_cc.r3; + tsai_cc.r6 = -tsai_cc.r6; + tsai_cc.r7 = -tsai_cc.r7; + tsai_cc.r8 = -tsai_cc.r8; + solve_RPY_transform (); + + if (!cc_compute_approximate_f_and_Tz()) + { + *iflag = -1; + return; + } + + if (tsai_cc.f < 0) + { + pytsai_raise("error - possible handedness problem with data"); + *iflag = -1; + return; + } + } + + /* now calculate the squared error assuming zero distortion */ + for (i = 0; i < tsai_cd.point_count; i++) { + /* convert from world coordinates to camera coordinates */ + /* Note: zw is implicitly assumed to be zero for these (coplanar) calculations */ + xc = tsai_cc.r1 * tsai_cd.xw[i] + tsai_cc.r2 * tsai_cd.yw[i] + tsai_cc.Tx; + yc = tsai_cc.r4 * tsai_cd.xw[i] + tsai_cc.r5 * tsai_cd.yw[i] + tsai_cc.Ty; + zc = tsai_cc.r7 * tsai_cd.xw[i] + tsai_cc.r8 * tsai_cd.yw[i] + Tz; + + /* convert from camera coordinates to undistorted sensor coordinates */ + Xu_1 = f * xc / zc; + Yu_1 = f * yc / zc; + + /* convert from distorted sensor coordinates to undistorted sensor coordinates */ + /* (already done, actually) */ + Xu_2 = Xd[i]; + Yu_2 = Yd[i]; + + /* record the error in the undistorted sensor coordinates */ + err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2); + } + + return; +} + + +/* pytsai: can fail; int return type is required. */ +int cc_five_parm_optimization_with_early_distortion_removal () +{ +#define NPARAMS 5 + + int i; + + /* Parameters needed by MINPACK's lmdif() */ + + integer m = tsai_cd.point_count; + integer n = NPARAMS; + doublereal x[NPARAMS]; + doublereal *fvec; + doublereal ftol = REL_SENSOR_TOLERANCE_ftol; + doublereal xtol = REL_PARAM_TOLERANCE_xtol; + doublereal gtol = ORTHO_TOLERANCE_gtol; + integer maxfev = MAXFEV; + doublereal epsfcn = EPSFCN; + doublereal diag[NPARAMS]; + integer mode = MODE; + doublereal factor = FACTOR; + integer nprint = 0; + integer info; + integer nfev; + doublereal *fjac; + integer ldfjac = m; + integer ipvt[NPARAMS]; + doublereal qtf[NPARAMS]; + doublereal wa1[NPARAMS]; + doublereal wa2[NPARAMS]; + doublereal wa3[NPARAMS]; + doublereal *wa4; + + /* allocate some workspace */ + if (( fvec = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + pytsai_raise("malloc: Cannot allocate workspace fvec"); + return 0; + } + + if (( fjac = (doublereal *) malloc ((unsigned int) m*n * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + pytsai_raise("malloc: Cannot allocate workspace fjac"); + return 0; + } + + if (( wa4 = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + free(fjac); + pytsai_raise("malloc: Cannot allocate workspace wa4"); + return 0; + } + + /* use the current calibration and camera constants as a starting point */ + x[0] = tsai_cc.f; + x[1] = tsai_cc.Tz; + x[2] = tsai_cc.kappa1; + x[3] = tsai_cp.Cx; + x[4] = tsai_cp.Cy; + + /* define optional scale factors for the parameters */ + if ( mode == 2 ) { + for (i = 0; i < NPARAMS; i++) + diag[i] = 1.0; /* some user-defined values */ + } + + /* perform the optimization */ + lmdif_ (cc_five_parm_optimization_with_early_distortion_removal_error, + &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn, + diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac, + ipvt, qtf, wa1, wa2, wa3, wa4); + /* check for pytsai error condition */ + if (info == -1) + { + free(fvec); + free(fjac); + free(wa4); + return 0; + } + /* TODO: Check for other error conditions. See lmdif.c for possible + * values of the info parameter. */ + + /* update the calibration and camera constants */ + tsai_cc.f = x[0]; + tsai_cc.Tz = x[1]; + tsai_cc.kappa1 = x[2]; + tsai_cp.Cx = x[3]; + tsai_cp.Cy = x[4]; + + /* release allocated workspace */ + free(fvec); + free(fjac); + free(wa4); + +#ifdef DEBUG + /* print the number of function calls during iteration */ + fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev); +#endif + + return 1; + +#undef NPARAMS +} + + +/************************************************************************/ +/* pytsai: cannot fail; void return type is fine. */ +void cc_nic_optimization_error (m_ptr, n_ptr, params, err) + integer *m_ptr; /* pointer to number of points to fit */ + integer *n_ptr; /* pointer to number of parameters */ + doublereal *params; /* vector of parameters */ + doublereal *err; /* vector of error from data */ +{ + int i; + + double xc, + yc, + zc, + Xd_, + Yd_, + Xu_1, + Yu_1, + Xu_2, + Yu_2, + distortion_factor, + Rx, + Ry, + Rz, + Tx, + Ty, + Tz, + kappa1, + f, + r1, + r2, + r4, + r5, + r7, + r8, + sa, + sb, + sg, + ca, + cb, + cg; + + Rx = params[0]; + Ry = params[1]; + Rz = params[2]; + Tx = params[3]; + Ty = params[4]; + Tz = params[5]; + kappa1 = params[6]; + f = params[7]; + + SINCOS (Rx, sa, ca); + SINCOS (Ry, sb, cb); + SINCOS (Rz, sg, cg); + r1 = cb * cg; + r2 = cg * sa * sb - ca * sg; + r4 = cb * sg; + r5 = sa * sb * sg + ca * cg; + r7 = -sb; + r8 = cb * sa; + + for (i = 0; i < tsai_cd.point_count; i++) { + /* convert from world coordinates to camera coordinates */ + /* Note: zw is implicitly assumed to be zero for these (coplanar) calculations */ + xc = r1 * tsai_cd.xw[i] + r2 * tsai_cd.yw[i] + Tx; + yc = r4 * tsai_cd.xw[i] + r5 * tsai_cd.yw[i] + Ty; + zc = r7 * tsai_cd.xw[i] + r8 * tsai_cd.yw[i] + Tz; + + /* convert from camera coordinates to undistorted sensor plane coordinates */ + Xu_1 = f * xc / zc; + Yu_1 = f * yc / zc; + + /* convert from 2D image coordinates to distorted sensor coordinates */ + Xd_ = tsai_cp.dpx * (tsai_cd.Xf[i] - tsai_cp.Cx) / tsai_cp.sx; + Yd_ = tsai_cp.dpy * (tsai_cd.Yf[i] - tsai_cp.Cy); + + /* convert from distorted sensor coordinates to undistorted sensor plane coordinates */ + distortion_factor = 1 + kappa1 * (SQR (Xd_) + SQR (Yd_)); + Xu_2 = Xd_ * distortion_factor; + Yu_2 = Yd_ * distortion_factor; + + /* record the error in the undistorted sensor coordinates */ + err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2); + } +} + + +/* pytsai: can fail; int return type is required. */ +int cc_nic_optimization () +{ +#define NPARAMS 8 + + int i; + + /* Parameters needed by MINPACK's lmdif() */ + + integer m = tsai_cd.point_count; + integer n = NPARAMS; + doublereal x[NPARAMS]; + doublereal *fvec; + doublereal ftol = REL_SENSOR_TOLERANCE_ftol; + doublereal xtol = REL_PARAM_TOLERANCE_xtol; + doublereal gtol = ORTHO_TOLERANCE_gtol; + integer maxfev = MAXFEV; + doublereal epsfcn = EPSFCN; + doublereal diag[NPARAMS]; + integer mode = MODE; + doublereal factor = FACTOR; + integer nprint = 0; + integer info; + integer nfev; + doublereal *fjac; + integer ldfjac = m; + integer ipvt[NPARAMS]; + doublereal qtf[NPARAMS]; + doublereal wa1[NPARAMS]; + doublereal wa2[NPARAMS]; + doublereal wa3[NPARAMS]; + doublereal *wa4; + + /* allocate some workspace */ + if (( fvec = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + pytsai_raise("malloc: Cannot allocate workspace fvec"); + return 0; + } + + if (( fjac = (doublereal *) malloc ((unsigned int) m*n * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + pytsai_raise("malloc: Cannot allocate workspace fjac"); + return 0; + } + + if (( wa4 = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + free(fjac); + pytsai_raise("malloc: Cannot allocate workspace wa4"); + return 0; + } + + /* use the current calibration and camera constants as a starting point */ + x[0] = tsai_cc.Rx; + x[1] = tsai_cc.Ry; + x[2] = tsai_cc.Rz; + x[3] = tsai_cc.Tx; + x[4] = tsai_cc.Ty; + x[5] = tsai_cc.Tz; + x[6] = tsai_cc.kappa1; + x[7] = tsai_cc.f; + + /* define optional scale factors for the parameters */ + if ( mode == 2 ) { + for (i = 0; i < NPARAMS; i++) + diag[i] = 1.0; /* some user-defined values */ + } + + /* perform the optimization */ + lmdif_ (cc_nic_optimization_error, + &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn, + diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac, + ipvt, qtf, wa1, wa2, wa3, wa4); + /* TODO: check for error conditions in info. */ + + /* update the calibration and camera constants */ + tsai_cc.Rx = x[0]; + tsai_cc.Ry = x[1]; + tsai_cc.Rz = x[2]; + apply_RPY_transform (); + + tsai_cc.Tx = x[3]; + tsai_cc.Ty = x[4]; + tsai_cc.Tz = x[5]; + tsai_cc.kappa1 = x[6]; + tsai_cc.f = x[7]; + + /* release allocated workspace */ + free(fvec); + free(fjac); + free(wa4); + +#ifdef DEBUG + /* print the number of function calls during iteration */ + fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev); +#endif + + return 1; + +#undef NPARAMS +} + + +/************************************************************************/ +/* pytsai: cannot fail; void return type is fine. */ +void cc_full_optimization_error (m_ptr, n_ptr, params, err) + integer *m_ptr; /* pointer to number of points to fit */ + integer *n_ptr; /* pointer to number of parameters */ + doublereal *params; /* vector of parameters */ + doublereal *err; /* vector of error from data */ +{ + int i; + + double xc, + yc, + zc, + Xd_, + Yd_, + Xu_1, + Yu_1, + Xu_2, + Yu_2, + distortion_factor, + Rx, + Ry, + Rz, + Tx, + Ty, + Tz, + kappa1, + f, + Cx, + Cy, + r1, + r2, + r4, + r5, + r7, + r8, + sa, + sb, + sg, + ca, + cb, + cg; + + Rx = params[0]; + Ry = params[1]; + Rz = params[2]; + Tx = params[3]; + Ty = params[4]; + Tz = params[5]; + kappa1 = params[6]; + f = params[7]; + Cx = params[8]; + Cy = params[9]; + + SINCOS (Rx, sa, ca); + SINCOS (Ry, sb, cb); + SINCOS (Rz, sg, cg); + r1 = cb * cg; + r2 = cg * sa * sb - ca * sg; + r4 = cb * sg; + r5 = sa * sb * sg + ca * cg; + r7 = -sb; + r8 = cb * sa; + + for (i = 0; i < tsai_cd.point_count; i++) { + /* convert from world coordinates to camera coordinates */ + /* Note: zw is implicitly assumed to be zero for these (coplanar) calculations */ + xc = r1 * tsai_cd.xw[i] + r2 * tsai_cd.yw[i] + Tx; + yc = r4 * tsai_cd.xw[i] + r5 * tsai_cd.yw[i] + Ty; + zc = r7 * tsai_cd.xw[i] + r8 * tsai_cd.yw[i] + Tz; + + /* convert from camera coordinates to undistorted sensor plane coordinates */ + Xu_1 = f * xc / zc; + Yu_1 = f * yc / zc; + + /* convert from 2D image coordinates to distorted sensor coordinates */ + Xd_ = tsai_cp.dpx * (tsai_cd.Xf[i] - Cx) / tsai_cp.sx; + Yd_ = tsai_cp.dpy * (tsai_cd.Yf[i] - Cy); + + /* convert from distorted sensor coordinates to undistorted sensor plane coordinates */ + distortion_factor = 1 + kappa1 * (SQR (Xd_) + SQR (Yd_)); + Xu_2 = Xd_ * distortion_factor; + Yu_2 = Yd_ * distortion_factor; + + /* record the error in the undistorted sensor coordinates */ + err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2); + } +} + + +/* pytsai: can fail; int return type is required. */ +int cc_full_optimization () +{ +#define NPARAMS 10 + + int i; + + /* Parameters needed by MINPACK's lmdif() */ + + integer m = tsai_cd.point_count; + integer n = NPARAMS; + doublereal x[NPARAMS]; + doublereal *fvec; + doublereal ftol = REL_SENSOR_TOLERANCE_ftol; + doublereal xtol = REL_PARAM_TOLERANCE_xtol; + doublereal gtol = ORTHO_TOLERANCE_gtol; + integer maxfev = MAXFEV; + doublereal epsfcn = EPSFCN; + doublereal diag[NPARAMS]; + integer mode = MODE; + doublereal factor = FACTOR; + integer nprint = 0; + integer info; + integer nfev; + doublereal *fjac; + integer ldfjac = m; + integer ipvt[NPARAMS]; + doublereal qtf[NPARAMS]; + doublereal wa1[NPARAMS]; + doublereal wa2[NPARAMS]; + doublereal wa3[NPARAMS]; + doublereal *wa4; + + /* allocate some workspace */ + if (( fvec = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + pytsai_raise("malloc: Cannot allocate workspace fvec"); + return 0; + } + + if (( fjac = (doublereal *) malloc ((unsigned int) m*n * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + pytsai_raise("malloc: Cannot allocate workspace fjac"); + return 0; + } + + if (( wa4 = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + free(fjac); + pytsai_raise("malloc: Cannot allocate workspace wa4"); + return 0; + } + + /* use the current calibration and camera constants as a starting point */ + x[0] = tsai_cc.Rx; + x[1] = tsai_cc.Ry; + x[2] = tsai_cc.Rz; + x[3] = tsai_cc.Tx; + x[4] = tsai_cc.Ty; + x[5] = tsai_cc.Tz; + x[6] = tsai_cc.kappa1; + x[7] = tsai_cc.f; + x[8] = tsai_cp.Cx; + x[9] = tsai_cp.Cy; + + /* define optional scale factors for the parameters */ + if ( mode == 2 ) { + for (i = 0; i < NPARAMS; i++) + diag[i] = 1.0; /* some user-defined values */ + } + + /* perform the optimization */ + lmdif_ (cc_full_optimization_error, + &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn, + diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac, + ipvt, qtf, wa1, wa2, wa3, wa4); + /* TODO: Check for error conditions (info parameter). See lmdif.c for + * possible values. */ + + /* update the calibration and camera constants */ + tsai_cc.Rx = x[0]; + tsai_cc.Ry = x[1]; + tsai_cc.Rz = x[2]; + apply_RPY_transform (); + + tsai_cc.Tx = x[3]; + tsai_cc.Ty = x[4]; + tsai_cc.Tz = x[5]; + tsai_cc.kappa1 = x[6]; + tsai_cc.f = x[7]; + tsai_cp.Cx = x[8]; + tsai_cp.Cy = x[9]; + + /* release allocated workspace */ + free(fvec); + free(fjac); + free(wa4); + +#ifdef DEBUG + /* print the number of function calls during iteration */ + fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev); +#endif + + return 1; + +#undef NPARAMS +} + + +/***********************************************************************\ +* Routines for noncoplanar camera calibration * +\***********************************************************************/ +/* pytsai: cannot fail; void return type is fine. */ +void ncc_compute_Xd_Yd_and_r_squared () +{ + int i; + + double Xd_, + Yd_; + + for (i = 0; i < tsai_cd.point_count; i++) { + Xd[i] = Xd_ = tsai_cp.dpx * (tsai_cd.Xf[i] - tsai_cp.Cx) / tsai_cp.sx; /* [mm] */ + Yd[i] = Yd_ = tsai_cp.dpy * (tsai_cd.Yf[i] - tsai_cp.Cy); /* [mm] */ + r_squared[i] = SQR (Xd_) + SQR (Yd_); /* [mm^2] */ + } +} + + +/* pytsai: can fail; int return type required. */ +int ncc_compute_U () +{ + int i; + + dmat M, + a, + b; + + M = newdmat (0, (tsai_cd.point_count - 1), 0, 6, &errno); + if (errno) { + pytsai_raise("ncc compute U: unable to allocate matrix M"); + return 0; + } + + a = newdmat (0, 6, 0, 0, &errno); + if (errno) { + freemat(M); + pytsai_raise("ncc compute U: unable to allocate vector a"); + return 0; + } + + b = newdmat (0, (tsai_cd.point_count - 1), 0, 0, &errno); + if (errno) { + freemat(M); + freemat(a); + pytsai_raise("ncc compute U: unable to allocate vector b"); + return 0; + } + + for (i = 0; i < tsai_cd.point_count; i++) { + M.el[i][0] = Yd[i] * tsai_cd.xw[i]; + M.el[i][1] = Yd[i] * tsai_cd.yw[i]; + M.el[i][2] = Yd[i] * tsai_cd.zw[i]; + M.el[i][3] = Yd[i]; + M.el[i][4] = -Xd[i] * tsai_cd.xw[i]; + M.el[i][5] = -Xd[i] * tsai_cd.yw[i]; + M.el[i][6] = -Xd[i] * tsai_cd.zw[i]; + b.el[i][0] = Xd[i]; + } + + if (solve_system (M, a, b)) { + freemat(M); + freemat(a); + freemat(b); + pytsai_raise("ncc compute U: error - non-coplanar calibration tried with data which may possibly be coplanar"); + return 0; + } + + U[0] = a.el[0][0]; + U[1] = a.el[1][0]; + U[2] = a.el[2][0]; + U[3] = a.el[3][0]; + U[4] = a.el[4][0]; + U[5] = a.el[5][0]; + U[6] = a.el[6][0]; + + freemat (M); + freemat (a); + freemat (b); + + return 1; +} + + +/* pytsai: cannot fail; void return type is fine. */ +void ncc_compute_Tx_and_Ty () +{ + int i, + far_point; + + double Tx, + Ty, + Ty_squared, + x, + y, + r1, + r2, + r3, + r4, + r5, + r6, + distance, + far_distance; + + /* first find the square of the magnitude of Ty */ + Ty_squared = 1 / (SQR (U[4]) + SQR (U[5]) + SQR (U[6])); + + /* find a point that is far from the image center */ + far_distance = 0; + far_point = 0; + for (i = 0; i < tsai_cd.point_count; i++) + if ((distance = r_squared[i]) > far_distance) { + far_point = i; + far_distance = distance; + } + + /* now find the sign for Ty */ + /* start by assuming Ty > 0 */ + Ty = sqrt (Ty_squared); + r1 = U[0] * Ty; + r2 = U[1] * Ty; + r3 = U[2] * Ty; + Tx = U[3] * Ty; + r4 = U[4] * Ty; + r5 = U[5] * Ty; + r6 = U[6] * Ty; + x = r1 * tsai_cd.xw[far_point] + r2 * tsai_cd.yw[far_point] + r3 * tsai_cd.zw[far_point] + Tx; + y = r4 * tsai_cd.xw[far_point] + r5 * tsai_cd.yw[far_point] + r6 * tsai_cd.zw[far_point] + Ty; + + /* flip Ty if we guessed wrong */ + if ((SIGNBIT (x) != SIGNBIT (Xd[far_point])) || + (SIGNBIT (y) != SIGNBIT (Yd[far_point]))) + Ty = -Ty; + + /* update the calibration constants */ + tsai_cc.Tx = U[3] * Ty; + tsai_cc.Ty = Ty; +} + + +/* pytsai: cannot fail; void return type is fine. */ +void ncc_compute_sx () +{ + tsai_cp.sx = sqrt (SQR (U[0]) + SQR (U[1]) + SQR (U[2])) * fabs (tsai_cc.Ty); +} + + +/* pytsai: cannot fail; void return type is fine. */ +void ncc_compute_R () +{ + double r1, + r2, + r3, + r4, + r5, + r6, + r7, + r8, + r9; + + r1 = U[0] * tsai_cc.Ty / tsai_cp.sx; + r2 = U[1] * tsai_cc.Ty / tsai_cp.sx; + r3 = U[2] * tsai_cc.Ty / tsai_cp.sx; + + r4 = U[4] * tsai_cc.Ty; + r5 = U[5] * tsai_cc.Ty; + r6 = U[6] * tsai_cc.Ty; + + /* use the outer product of the first two rows to get the last row */ + r7 = r2 * r6 - r3 * r5; + r8 = r3 * r4 - r1 * r6; + r9 = r1 * r5 - r2 * r4; + + /* update the calibration constants */ + tsai_cc.r1 = r1; + tsai_cc.r2 = r2; + tsai_cc.r3 = r3; + tsai_cc.r4 = r4; + tsai_cc.r5 = r5; + tsai_cc.r6 = r6; + tsai_cc.r7 = r7; + tsai_cc.r8 = r8; + tsai_cc.r9 = r9; + + /* fill in tsai_cc.Rx, tsai_cc.Ry and tsai_cc.Rz */ + solve_RPY_transform (); +} + + +/* pytsai: cannot fail; void return type is fine. */ +void ncc_compute_better_R () +{ + double r1, + r2, + r3, + r4, + r5, + r6, + r7, + sa, + ca, + sb, + cb, + sg, + cg; + + r1 = U[0] * tsai_cc.Ty / tsai_cp.sx; + r2 = U[1] * tsai_cc.Ty / tsai_cp.sx; + r3 = U[2] * tsai_cc.Ty / tsai_cp.sx; + + r4 = U[4] * tsai_cc.Ty; + r5 = U[5] * tsai_cc.Ty; + r6 = U[6] * tsai_cc.Ty; + + /* use the outer product of the first two rows to get the last row */ + r7 = r2 * r6 - r3 * r5; + + /* now find the RPY angles corresponding to the estimated rotation matrix */ + tsai_cc.Rz = atan2 (r4, r1); + + SINCOS (tsai_cc.Rz, sg, cg); + + tsai_cc.Ry = atan2 (-r7, r1 * cg + r4 * sg); + + tsai_cc.Rx = atan2 (r3 * sg - r6 * cg, r5 * cg - r2 * sg); + + SINCOS (tsai_cc.Rx, sa, ca); + + SINCOS (tsai_cc.Ry, sb, cb); + + /* now generate a more orthonormal rotation matrix from the RPY angles */ + tsai_cc.r1 = cb * cg; + tsai_cc.r2 = cg * sa * sb - ca * sg; + tsai_cc.r3 = sa * sg + ca * cg * sb; + tsai_cc.r4 = cb * sg; + tsai_cc.r5 = sa * sb * sg + ca * cg; + tsai_cc.r6 = ca * sb * sg - cg * sa; + tsai_cc.r7 = -sb; + tsai_cc.r8 = cb * sa; + tsai_cc.r9 = ca * cb; +} + + +/* pytsai: can fail; int return type required. */ +int ncc_compute_approximate_f_and_Tz () +{ + int i; + + dmat M, + a, + b; + + M = newdmat (0, (tsai_cd.point_count - 1), 0, 1, &errno); + if (errno) { + pytsai_raise("ncc compute apx: unable to allocate matrix M"); + return 0; + } + + a = newdmat (0, 1, 0, 0, &errno); + if (errno) { + freemat(M); + pytsai_raise("ncc compute apx: unable to allocate vector a"); + return 0; + } + + b = newdmat (0, (tsai_cd.point_count - 1), 0, 0, &errno); + if (errno) { + freemat(M); + freemat(a); + pytsai_raise("ncc compute apx: unable to allocate vector b"); + return 0; + } + + for (i = 0; i < tsai_cd.point_count; i++) { + M.el[i][0] = tsai_cc.r4 * tsai_cd.xw[i] + tsai_cc.r5 * tsai_cd.yw[i] + tsai_cc.r6 * tsai_cd.zw[i] + tsai_cc.Ty; + M.el[i][1] = -Yd[i]; + b.el[i][0] = (tsai_cc.r7 * tsai_cd.xw[i] + tsai_cc.r8 * tsai_cd.yw[i] + tsai_cc.r9 * tsai_cd.zw[i]) * Yd[i]; + } + + if (solve_system (M, a, b)) { + freemat(M); + freemat(a); + freemat(b); + pytsai_raise("ncc compute apx: unable to solve system Ma=b"); + return 0; + } + + /* update the calibration constants */ + tsai_cc.f = a.el[0][0]; + tsai_cc.Tz = a.el[1][0]; + tsai_cc.kappa1 = 0.0; /* this is the assumption that our calculation was made under */ + + freemat (M); + freemat (a); + freemat (b); + + return 1; +} + + +/************************************************************************/ +/* pytsai: cannot fail; void return type is fine. */ +void ncc_compute_exact_f_and_Tz_error (m_ptr, n_ptr, params, err) + integer *m_ptr; /* pointer to number of points to fit */ + integer *n_ptr; /* pointer to number of parameters */ + doublereal *params; /* vector of parameters */ + doublereal *err; /* vector of error from data */ +{ + int i; + + double xc, + yc, + zc, + Xu_1, + Yu_1, + Xu_2, + Yu_2, + distortion_factor, + f, + Tz, + kappa1; + + f = params[0]; + Tz = params[1]; + kappa1 = params[2]; + + for (i = 0; i < tsai_cd.point_count; i++) { + /* convert from world coordinates to camera coordinates */ + xc = tsai_cc.r1 * tsai_cd.xw[i] + tsai_cc.r2 * tsai_cd.yw[i] + tsai_cc.r3 * tsai_cd.zw[i] + tsai_cc.Tx; + yc = tsai_cc.r4 * tsai_cd.xw[i] + tsai_cc.r5 * tsai_cd.yw[i] + tsai_cc.r6 * tsai_cd.zw[i] + tsai_cc.Ty; + zc = tsai_cc.r7 * tsai_cd.xw[i] + tsai_cc.r8 * tsai_cd.yw[i] + tsai_cc.r9 * tsai_cd.zw[i] + Tz; + + /* convert from camera coordinates to undistorted sensor coordinates */ + Xu_1 = f * xc / zc; + Yu_1 = f * yc / zc; + + /* convert from distorted sensor coordinates to undistorted sensor coordinates */ + distortion_factor = 1 + kappa1 * r_squared[i]; + Xu_2 = Xd[i] * distortion_factor; + Yu_2 = Yd[i] * distortion_factor; + + /* record the error in the undistorted sensor coordinates */ + err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2); + } +} + + +/* pytsai: can fail; int return type is required. */ +int ncc_compute_exact_f_and_Tz () +{ +#define NPARAMS 3 + + int i; + + /* Parameters needed by MINPACK's lmdif() */ + + integer m = tsai_cd.point_count; + integer n = NPARAMS; + doublereal x[NPARAMS]; + doublereal *fvec; + doublereal ftol = REL_SENSOR_TOLERANCE_ftol; + doublereal xtol = REL_PARAM_TOLERANCE_xtol; + doublereal gtol = ORTHO_TOLERANCE_gtol; + integer maxfev = MAXFEV; + doublereal epsfcn = EPSFCN; + doublereal diag[NPARAMS]; + integer mode = MODE; + doublereal factor = FACTOR; + integer nprint = 0; + integer info; + integer nfev; + doublereal *fjac; + integer ldfjac = m; + integer ipvt[NPARAMS]; + doublereal qtf[NPARAMS]; + doublereal wa1[NPARAMS]; + doublereal wa2[NPARAMS]; + doublereal wa3[NPARAMS]; + doublereal *wa4; + + /* allocate some workspace */ + if (( fvec = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + pytsai_raise("malloc: Cannot allocate workspace fvec"); + return 0; + } + + if (( fjac = (doublereal *) malloc ((unsigned int) m*n * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + pytsai_raise("malloc: Cannot allocate workspace fjac"); + return 0; + } + + if (( wa4 = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + free(fjac); + pytsai_raise("malloc: Cannot allocate workspace wa4"); + return 0; + } + + /* use the current calibration constants as an initial guess */ + x[0] = tsai_cc.f; + x[1] = tsai_cc.Tz; + x[2] = tsai_cc.kappa1; + + /* define optional scale factors for the parameters */ + if ( mode == 2 ) { + for (i = 0; i < NPARAMS; i++) + diag[i] = 1.0; /* some user-defined values */ + } + + /* perform the optimization */ + lmdif_ (ncc_compute_exact_f_and_Tz_error, + &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn, + diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac, + ipvt, qtf, wa1, wa2, wa3, wa4); + /* TODO: Check for error conditions (info parameter). See lmbif.c + * for possible values. */ + + /* update the calibration constants */ + tsai_cc.f = x[0]; + tsai_cc.Tz = x[1]; + tsai_cc.kappa1 = x[2]; + + /* release allocated workspace */ + free(fvec); + free(fjac); + free(wa4); + +#ifdef DEBUG + /* print the number of function calls during iteration */ + fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev); +#endif + + return 1; + +#undef NPARAMS +} + + +/************************************************************************/ +/* pytsai: can fail; int return type is required. */ +int ncc_three_parm_optimization () +{ + ncc_compute_Xd_Yd_and_r_squared (); + + if (!ncc_compute_U()) + return 0; + + ncc_compute_Tx_and_Ty (); + + ncc_compute_sx (); + + ncc_compute_Xd_Yd_and_r_squared (); + + ncc_compute_better_R (); + + if (!ncc_compute_approximate_f_and_Tz()) + return 0; + + if (tsai_cc.f < 0) { + /* try the other solution for the orthonormal matrix */ + tsai_cc.r3 = -tsai_cc.r3; + tsai_cc.r6 = -tsai_cc.r6; + tsai_cc.r7 = -tsai_cc.r7; + tsai_cc.r8 = -tsai_cc.r8; + solve_RPY_transform (); + + if (!ncc_compute_approximate_f_and_Tz()) + return 0; + + if (tsai_cc.f < 0) { + pytsai_raise("error - possible handedness problem with data"); + return 0; + } + } + + if (!ncc_compute_exact_f_and_Tz()) + return 0; + + return 1; +} + + +/************************************************************************/ +/* pytsai: cannot fail; void return type is fine. */ +void ncc_nic_optimization_error (m_ptr, n_ptr, params, err) + integer *m_ptr; /* pointer to number of points to fit */ + integer *n_ptr; /* pointer to number of parameters */ + doublereal *params; /* vector of parameters */ + doublereal *err; /* vector of error from data */ +{ + int i; + + double xc, + yc, + zc, + Xd_, + Yd_, + Xu_1, + Yu_1, + Xu_2, + Yu_2, + distortion_factor, + Rx, + Ry, + Rz, + Tx, + Ty, + Tz, + kappa1, + sx, + f, + r1, + r2, + r3, + r4, + r5, + r6, + r7, + r8, + r9, + sa, + sb, + sg, + ca, + cb, + cg; + + Rx = params[0]; + Ry = params[1]; + Rz = params[2]; + Tx = params[3]; + Ty = params[4]; + Tz = params[5]; + kappa1 = params[6]; + f = params[7]; + sx = params[8]; + + SINCOS (Rx, sa, ca); + SINCOS (Ry, sb, cb); + SINCOS (Rz, sg, cg); + r1 = cb * cg; + r2 = cg * sa * sb - ca * sg; + r3 = sa * sg + ca * cg * sb; + r4 = cb * sg; + r5 = sa * sb * sg + ca * cg; + r6 = ca * sb * sg - cg * sa; + r7 = -sb; + r8 = cb * sa; + r9 = ca * cb; + + for (i = 0; i < tsai_cd.point_count; i++) { + /* convert from world coordinates to camera coordinates */ + xc = r1 * tsai_cd.xw[i] + r2 * tsai_cd.yw[i] + r3 * tsai_cd.zw[i] + Tx; + yc = r4 * tsai_cd.xw[i] + r5 * tsai_cd.yw[i] + r6 * tsai_cd.zw[i] + Ty; + zc = r7 * tsai_cd.xw[i] + r8 * tsai_cd.yw[i] + r9 * tsai_cd.zw[i] + Tz; + + /* convert from camera coordinates to undistorted sensor plane coordinates */ + Xu_1 = f * xc / zc; + Yu_1 = f * yc / zc; + + /* convert from 2D image coordinates to distorted sensor coordinates */ + Xd_ = tsai_cp.dpx * (tsai_cd.Xf[i] - tsai_cp.Cx) / sx; + Yd_ = tsai_cp.dpy * (tsai_cd.Yf[i] - tsai_cp.Cy); + + /* convert from distorted sensor coordinates to undistorted sensor plane coordinates */ + distortion_factor = 1 + kappa1 * (SQR (Xd_) + SQR (Yd_)); + Xu_2 = Xd_ * distortion_factor; + Yu_2 = Yd_ * distortion_factor; + + /* record the error in the undistorted sensor coordinates */ + err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2); + } +} + + +/* pytsai: can fail; int return type is required. */ +int ncc_nic_optimization () +{ +#define NPARAMS 9 + + int i; + + /* Parameters needed by MINPACK's lmdif() */ + + integer m = tsai_cd.point_count; + integer n = NPARAMS; + doublereal x[NPARAMS]; + doublereal *fvec; + doublereal ftol = REL_SENSOR_TOLERANCE_ftol; + doublereal xtol = REL_PARAM_TOLERANCE_xtol; + doublereal gtol = ORTHO_TOLERANCE_gtol; + integer maxfev = MAXFEV; + doublereal epsfcn = EPSFCN; + doublereal diag[NPARAMS]; + integer mode = MODE; + doublereal factor = FACTOR; + integer nprint = 0; + integer info; + integer nfev; + doublereal *fjac; + integer ldfjac = m; + integer ipvt[NPARAMS]; + doublereal qtf[NPARAMS]; + doublereal wa1[NPARAMS]; + doublereal wa2[NPARAMS]; + doublereal wa3[NPARAMS]; + doublereal *wa4; + + /* allocate some workspace */ + if (( fvec = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + pytsai_raise("malloc: Cannot allocate workspace fvec"); + return 0; + } + + if (( fjac = (doublereal *) malloc ((unsigned int) m*n * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + pytsai_raise("malloc: Cannot allocate workspace fjac"); + return 0; + } + + if (( wa4 = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + free(fjac); + pytsai_raise("malloc: Cannot allocate workspace wa4"); + return 0; + } + + /* use the current calibration and camera constants as a starting point */ + x[0] = tsai_cc.Rx; + x[1] = tsai_cc.Ry; + x[2] = tsai_cc.Rz; + x[3] = tsai_cc.Tx; + x[4] = tsai_cc.Ty; + x[5] = tsai_cc.Tz; + x[6] = tsai_cc.kappa1; + x[7] = tsai_cc.f; + x[8] = tsai_cp.sx; + + /* define optional scale factors for the parameters */ + if ( mode == 2 ) { + for (i = 0; i < NPARAMS; i++) + diag[i] = 1.0; /* some user-defined values */ + } + + /* perform the optimization */ + lmdif_ (ncc_nic_optimization_error, + &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn, + diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac, + ipvt, qtf, wa1, wa2, wa3, wa4); + /* TODO: Check for error conditions (info parameter). See lmdif.c for + * possible values of the info parameter. */ + + /* update the calibration and camera constants */ + tsai_cc.Rx = x[0]; + tsai_cc.Ry = x[1]; + tsai_cc.Rz = x[2]; + apply_RPY_transform (); + + tsai_cc.Tx = x[3]; + tsai_cc.Ty = x[4]; + tsai_cc.Tz = x[5]; + tsai_cc.kappa1 = x[6]; + tsai_cc.f = x[7]; + tsai_cp.sx = x[8]; + + /* release allocated workspace */ + free(fvec); + free(fjac); + free(wa4); + +#ifdef DEBUG + /* print the number of function calls during iteration */ + fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev); +#endif + + return 1; + +#undef NPARAMS +} + + +/************************************************************************/ +/* pytsai: cannot fail; void return type is fine. */ +void ncc_full_optimization_error (m_ptr, n_ptr, params, err) + integer *m_ptr; /* pointer to number of points to fit */ + integer *n_ptr; /* pointer to number of parameters */ + doublereal *params; /* vector of parameters */ + doublereal *err; /* vector of error from data */ +{ + int i; + + double xc, + yc, + zc, + Xd_, + Yd_, + Xu_1, + Yu_1, + Xu_2, + Yu_2, + distortion_factor, + Rx, + Ry, + Rz, + Tx, + Ty, + Tz, + kappa1, + sx, + f, + Cx, + Cy, + r1, + r2, + r3, + r4, + r5, + r6, + r7, + r8, + r9, + sa, + sb, + sg, + ca, + cb, + cg; + + Rx = params[0]; + Ry = params[1]; + Rz = params[2]; + Tx = params[3]; + Ty = params[4]; + Tz = params[5]; + kappa1 = params[6]; + f = params[7]; + sx = params[8]; + Cx = params[9]; + Cy = params[10]; + + SINCOS (Rx, sa, ca); + SINCOS (Ry, sb, cb); + SINCOS (Rz, sg, cg); + r1 = cb * cg; + r2 = cg * sa * sb - ca * sg; + r3 = sa * sg + ca * cg * sb; + r4 = cb * sg; + r5 = sa * sb * sg + ca * cg; + r6 = ca * sb * sg - cg * sa; + r7 = -sb; + r8 = cb * sa; + r9 = ca * cb; + + for (i = 0; i < tsai_cd.point_count; i++) { + /* convert from world coordinates to camera coordinates */ + xc = r1 * tsai_cd.xw[i] + r2 * tsai_cd.yw[i] + r3 * tsai_cd.zw[i] + Tx; + yc = r4 * tsai_cd.xw[i] + r5 * tsai_cd.yw[i] + r6 * tsai_cd.zw[i] + Ty; + zc = r7 * tsai_cd.xw[i] + r8 * tsai_cd.yw[i] + r9 * tsai_cd.zw[i] + Tz; + + /* convert from camera coordinates to undistorted sensor plane coordinates */ + Xu_1 = f * xc / zc; + Yu_1 = f * yc / zc; + + /* convert from 2D image coordinates to distorted sensor coordinates */ + Xd_ = tsai_cp.dpx * (tsai_cd.Xf[i] - Cx) / sx; + Yd_ = tsai_cp.dpy * (tsai_cd.Yf[i] - Cy); + + /* convert from distorted sensor coordinates to undistorted sensor plane coordinates */ + distortion_factor = 1 + kappa1 * (SQR (Xd_) + SQR (Yd_)); + Xu_2 = Xd_ * distortion_factor; + Yu_2 = Yd_ * distortion_factor; + + /* record the error in the undistorted sensor coordinates */ + err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2); + } +} + + +/* pytsai: can fail; int return type is required. */ +int ncc_full_optimization () +{ +#define NPARAMS 11 + + int i; + + /* Parameters needed by MINPACK's lmdif() */ + + integer m = tsai_cd.point_count; + integer n = NPARAMS; + doublereal x[NPARAMS]; + doublereal *fvec; + doublereal ftol = REL_SENSOR_TOLERANCE_ftol; + doublereal xtol = REL_PARAM_TOLERANCE_xtol; + doublereal gtol = ORTHO_TOLERANCE_gtol; + integer maxfev = MAXFEV; + doublereal epsfcn = EPSFCN; + doublereal diag[NPARAMS]; + integer mode = MODE; + doublereal factor = FACTOR; + integer nprint = 0; + integer info; + integer nfev; + doublereal *fjac; + integer ldfjac = m; + integer ipvt[NPARAMS]; + doublereal qtf[NPARAMS]; + doublereal wa1[NPARAMS]; + doublereal wa2[NPARAMS]; + doublereal wa3[NPARAMS]; + doublereal *wa4; + + /* allocate some workspace */ + if (( fvec = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + pytsai_raise("malloc: Cannot allocate workspace fvec"); + return 0; + } + + if (( fjac = (doublereal *) malloc ((unsigned int) m*n * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + pytsai_raise("malloc: Cannot allocate workspace fjac"); + return 0; + } + + if (( wa4 = (doublereal *) malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + free(fvec); + free(fjac); + pytsai_raise("malloc: Cannot allocate workspace wa4"); + return 0; + } + + /* use the current calibration and camera constants as a starting point */ + x[0] = tsai_cc.Rx; + x[1] = tsai_cc.Ry; + x[2] = tsai_cc.Rz; + x[3] = tsai_cc.Tx; + x[4] = tsai_cc.Ty; + x[5] = tsai_cc.Tz; + x[6] = tsai_cc.kappa1; + x[7] = tsai_cc.f; + x[8] = tsai_cp.sx; + x[9] = tsai_cp.Cx; + x[10] = tsai_cp.Cy; + + /* define optional scale factors for the parameters */ + if ( mode == 2 ) { + for (i = 0; i < NPARAMS; i++) + diag[i] = 1.0; /* some user-defined values */ + } + + /* perform the optimization */ + lmdif_ (ncc_full_optimization_error, + &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn, + diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac, + ipvt, qtf, wa1, wa2, wa3, wa4); + /* TODO: Check for error conditions (info parameter). See lmdif.c for + * possible values. */ + + /* update the calibration and camera constants */ + tsai_cc.Rx = x[0]; + tsai_cc.Ry = x[1]; + tsai_cc.Rz = x[2]; + apply_RPY_transform (); + + tsai_cc.Tx = x[3]; + tsai_cc.Ty = x[4]; + tsai_cc.Tz = x[5]; + tsai_cc.kappa1 = x[6]; + tsai_cc.f = x[7]; + tsai_cp.sx = x[8]; + tsai_cp.Cx = x[9]; + tsai_cp.Cy = x[10]; + + /* release allocated workspace */ + free(fvec); + free(fjac); + free(wa4); + +#ifdef DEBUG + /* print the number of function calls during iteration */ + fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev); +#endif + + return 1; + +#undef NPARAMS +} + + +/************************************************************************/ + +/* pytsai: can fail; int return type is required. */ +int coplanar_calibration () +{ + /* just do the basic 3 parameter (Tz, f, kappa1) optimization */ + return cc_three_parm_optimization (); +} + + +/* pytsai: can fail; int return type is required. */ +int coplanar_calibration_with_full_optimization () +{ + /* start with a 3 parameter (Tz, f, kappa1) optimization */ + if (!cc_three_parm_optimization()) + return 0; + + /* do a 5 parameter (Tz, f, kappa1, Cx, Cy) optimization */ + if (!cc_five_parm_optimization_with_late_distortion_removal()) + return 0; + + /* do a better 5 parameter (Tz, f, kappa1, Cx, Cy) optimization */ + if (!cc_five_parm_optimization_with_early_distortion_removal()) + return 0; + + /* do a full optimization minus the image center */ + if (!cc_nic_optimization()) + return 0; + + /* do a full optimization including the image center */ + if (!cc_full_optimization()) + return 0; + + return 1; +} + + +/* pytsai: can fail; int return type is required. */ +int noncoplanar_calibration () +{ + /* just do the basic 3 parameter (Tz, f, kappa1) optimization */ + return ncc_three_parm_optimization (); +} + + +/* pytsai: can fail; int return type is required. */ +int noncoplanar_calibration_with_full_optimization () +{ + /* start with a 3 parameter (Tz, f, kappa1) optimization */ + if (!ncc_three_parm_optimization()) + return 0; + + /* do a full optimization minus the image center */ + if (!ncc_nic_optimization()) + return 0; + + /* do a full optimization including the image center */ + if (!ncc_full_optimization()) + return 0; + + return 1; +} diff --git a/others/pyTsai/src/tsai/cal_main.h b/others/pyTsai/src/tsai/cal_main.h new file mode 100644 index 0000000..97b2cc3 --- /dev/null +++ b/others/pyTsai/src/tsai/cal_main.h @@ -0,0 +1,231 @@ +/** + * cal_main.h + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/****************************************************************************\ +* * +* This file contains operating constants, data structure definitions and I/O * +* variable declarations for the calibration routines contained in the file * +* cal_main.c * +* * +* An explanation of the basic algorithms and description of the variables * +* can be found in "An Efficient and Accurate Camera Calibration Technique * +* for 3D Machine Vision", Roger Y. Tsai, Proceedings of IEEE Conference on * +* Computer Vision and Pattern Recognition, 1986, pages 364-374. This work * +* also appears in several other publications under Roger Tsai's name. * +* * +* * +* Notation * +* -------- * +* * +* The camera's X axis runs along increasing column coordinates in the * +* image/frame. The Y axis runs along increasing row coordinates. * +* * +* pix == image/frame grabber picture element * +* sel == camera sensor element * +* * +* * +* History * +* ------- * +* * +* 03-Nov-2004 Jonathan Merritt (j.merritt@pgrad.unimelb.edu.au) at * +* The University of Melbourne. * +* * +* 01-Apr-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN * +* Filename changes for DOS port. * +* * +* 04-Jun-94 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN * +* Added alternate macro definitions for less common math functions. * +* * +* 25-Mar-94 Torfi Thorhallsson (torfit@verk.hi.is) at the University of * +* Iceland * +* Added definitions of parameters controlling MINPACK's lmdif() * +* optimization routine. * +* * +* 30-Nov-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * +* Added declaration for camera type. * +* * +* 07-Feb-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * +* Original implementation. * +* * +* * +\****************************************************************************/ + +#ifndef CAL_MAIN_H +#define CAL_MAIN_H + +/* Maximum number of data points allowed */ +#define MAX_POINTS 500 + +/* An arbitrary tolerance factor */ +#define EPSILON 1.0E-8 + +/* Commonly used macros */ +#define ABS(a) (((a) > 0) ? (a) : -(a)) +#define SIGNBIT(a) (((a) > 0) ? 0 : 1) +#define SQR(a) ((a) * (a)) +#define CUB(a) ((a)*(a)*(a)) +#define MAX(a,b) (((a) > (b)) ? (a) : (b)) +#define MIN(a,b) (((a) < (b)) ? (a) : (b)) + +/* Some math libraries may not have the sincos() routine */ +#ifdef _SINCOS_ +void sincos(); +#define SINCOS(x,s,c) sincos(x,&s,&c) +#else +double sin(), cos(); +#define SINCOS(x,s,c) s=sin(x);c=cos(x) +#endif + + +/* Parameters controlling MINPACK's lmdif() optimization routine. */ +/* See the file lmdif.f for definitions of each parameter. */ +#define REL_SENSOR_TOLERANCE_ftol 1.0E-5 /* [pix] */ +#define REL_PARAM_TOLERANCE_xtol 1.0E-7 +#define ORTHO_TOLERANCE_gtol 0.0 +#define MAXFEV (1000*n) +#define EPSFCN 1.0E-16 /* Do not set to zero! */ +#define MODE 1 /* variables are scalled internally */ +#define FACTOR 100.0 + +/****************************************************************************\ +* * +* Camera parameters are usually the fixed parameters of the given camera * +* system, typically obtained from manufacturers specifications. * +* * +* Cy and Cy (the center of radial lens distortion), may be calibrated * +* separately or as part of the coplanar/noncoplanar calibration. * +* The same with sx (x scale uncertainty factor). * +* * +\****************************************************************************/ +struct camera_parameters { + double Ncx; /* [sel] Number of sensor elements in camera's x direction */ + double Nfx; /* [pix] Number of pixels in frame grabber's x direction */ + double dx; /* [mm/sel] X dimension of camera's sensor element (in mm) */ + double dy; /* [mm/sel] Y dimension of camera's sensor element (in mm) */ + double dpx; /* [mm/pix] Effective X dimension of pixel in frame grabber */ + double dpy; /* [mm/pix] Effective Y dimension of pixel in frame grabber */ + double Cx; /* [pix] Z axis intercept of camera coordinate system */ + double Cy; /* [pix] Z axis intercept of camera coordinate system */ + double sx; /* [] Scale factor to compensate for any error in dpx */ +}; + +/****************************************************************************\ +* * +* Calibration data consists of the (x,y,z) world coordinates of a feature * +* point (in mm) and the corresponding coordinates (Xf,Yf) (in pixels) of the * +* feature point in the image. Point count is the number of points in the * +* data set. * +* * +* * +* Coplanar calibration: * +* * +* For coplanar calibration the z world coordinate of the data must be zero. * +* In addition, for numerical stability the coordinates of the feature points * +* should be placed at some arbitrary distance from the origin (0,0,0) of the * +* world coordinate system. Finally, the plane containing the feature points * +* should not be parallel to the imaging plane. A relative angle of 30 * +* degrees is recommended. * +* * +* * +* Noncoplanar calibration: * +* * +* For noncoplanar calibration the data must not lie in a single plane. * +* * +\****************************************************************************/ +struct calibration_data { + int point_count; /* [points] */ + double xw[MAX_POINTS]; /* [mm] */ + double yw[MAX_POINTS]; /* [mm] */ + double zw[MAX_POINTS]; /* [mm] */ + double Xf[MAX_POINTS]; /* [pix] */ + double Yf[MAX_POINTS]; /* [pix] */ +}; + + +/****************************************************************************\ +* * +* Calibration constants are the model constants that are determined from the * +* calibration data. * +* * +\****************************************************************************/ +struct calibration_constants { + double f; /* [mm] */ + double kappa1; /* [1/mm^2] */ + double p1; /* [1/mm] */ + double p2; /* [1/mm] */ + double Tx; /* [mm] */ + double Ty; /* [mm] */ + double Tz; /* [mm] */ + double Rx; /* [rad] */ + double Ry; /* [rad] */ + double Rz; /* [rad] */ + double r1; /* [] */ + double r2; /* [] */ + double r3; /* [] */ + double r4; /* [] */ + double r5; /* [] */ + double r6; /* [] */ + double r7; /* [] */ + double r8; /* [] */ + double r9; /* [] */ +}; + +/* External declarations for variables used by the subroutines for I/O */ +extern struct camera_parameters tsai_cp; +extern struct calibration_data tsai_cd; +extern struct calibration_constants tsai_cc; +//extern char camera_type[]; + +/* Forward declarations for the calibration routines */ +#if 0 /* not needed for the Python version. */ +void initialize_photometrics_parms (); +void initialize_general_imaging_mos5300_matrox_parms (); +void initialize_panasonic_gp_mf702_matrox_parms (); +void initialize_sony_xc77_matrox_parms (); +void initialize_sony_xc57_androx_parms (); +#endif + +int coplanar_calibration (); +int coplanar_calibration_with_full_optimization (); + +int noncoplanar_calibration (); +int noncoplanar_calibration_with_full_optimization (); + +int coplanar_extrinsic_parameter_estimation (); +int noncoplanar_extrinsic_parameter_estimation (); + +void world_coord_to_image_coord (); +void image_coord_to_world_coord (); +void world_coord_to_camera_coord (); +void camera_coord_to_world_coord (); +void distorted_to_undistorted_sensor_coord (); +void undistorted_to_distorted_sensor_coord (); +void distorted_to_undistorted_image_coord (); +void undistorted_to_distorted_image_coord (); + +void distorted_image_plane_error_stats (); +void undistorted_image_plane_error_stats (); +void object_space_error_stats (); +void normalized_calibration_error (); + +void solve_RPY_transform (); +void apply_RPY_transform (); + +#endif /* CAL_MAIN_H */ + diff --git a/others/pyTsai/src/tsai/cal_tran.c b/others/pyTsai/src/tsai/cal_tran.c new file mode 100644 index 0000000..cfe7ca0 --- /dev/null +++ b/others/pyTsai/src/tsai/cal_tran.c @@ -0,0 +1,387 @@ +/** + * cal_tran.c + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/****************************************************************************\ +* * +* This file contains routines for transforming between the different * +* coordinate systems used in Tsai's perspective projection camera model. * +* The routines are: * +* * +* world_coord_to_image_coord () * +* image_coord_to_world_coord () * +* world_coord_to_camera_coord () * +* camera_coord_to_world_coord () * +* distorted_to_undistorted_sensor_coord () * +* undistorted_to_distorted_sensor_coord () * +* distorted_to_undistorted_image_coord () * +* undistorted_to_distorted_image_coord () * +* * +* The routines make use of the calibrated camera parameters and calibration * +* constants contained in the two external data structures tsai_cp and tsai_cc. * +* * +* Notation * +* -------- * +* * +* The camera's X axis runs along increasing column coordinates in the * +* image/frame. The Y axis runs along increasing row coordinates. * +* All 3D coordinates are right-handed. * +* * +* pix == image/frame grabber picture element * +* sel == camera sensor element * +* * +* * +* History * +* ------- * +* * +* 18-Oct-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN * +* Added check in undistorted_to_distorted_sensor_coord () for * +* situation where an undistorted sensor point maps to the maximum * +* barrel distortion radius. * +* * +* 18-May-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN * +* Split out from the cal_main.c file. * +* Fix the CBRT routine so it handles negative arguments properly. * +* * +\****************************************************************************/ +#include +#include +#include "cal_main.h" + + +extern struct camera_parameters tsai_cp; +extern struct calibration_constants tsai_cc; + + +#define SQRT(x) sqrt(fabs(x)) + + +/************************************************************************/ +/* This cube root routine handles negative arguments (unlike cbrt). */ + +double CBRT (x) + double x; +{ + double pow (); + + if (x == 0) + return (0); + else if (x > 0) + return (pow (x, (double) 1.0 / 3.0)); + else + return (-pow (-x, (double) 1.0 / 3.0)); +} + + +/************************************************************************/ +/* + This routine converts from undistorted to distorted sensor coordinates. + The technique involves algebraically solving the cubic polynomial + + Ru = Rd * (1 + kappa1 * Rd**2) + + using the Cardan method. + + Note: for kappa1 < 0 the distorted sensor plane extends out to a + maximum barrel distortion radius of Rd = sqrt (-1/(3 * kappa1)). + + To see the logic used in this routine try graphing the above + polynomial for positive and negative kappa1's +*/ + +void undistorted_to_distorted_sensor_coord (Xu, Yu, Xd, Yd) + double Xu, + Yu, + *Xd, + *Yd; +{ +#define SQRT3 1.732050807568877293527446341505872366943 + + double Ru, + Rd, + lambda, + c, + d, + Q, + R, + D, + S, + T, + sinT, + cosT; + + if (((Xu == 0) && (Yu == 0)) || (tsai_cc.kappa1 == 0)) { + *Xd = Xu; + *Yd = Yu; + return; + } + + Ru = hypot (Xu, Yu); /* SQRT(Xu*Xu+Yu*Yu) */ + + c = 1 / tsai_cc.kappa1; + d = -c * Ru; + + Q = c / 3; + R = -d / 2; + D = CUB (Q) + SQR (R); + + if (D >= 0) { /* one real root */ + D = SQRT (D); + S = CBRT (R + D); + T = CBRT (R - D); + Rd = S + T; + + if (Rd < 0) { + Rd = SQRT (-1 / (3 * tsai_cc.kappa1)); + fprintf (stderr, "\nWarning: undistorted image point to distorted image point mapping limited by\n"); + fprintf (stderr, " maximum barrel distortion radius of %lf\n", Rd); + fprintf (stderr, " (Xu = %lf, Yu = %lf) -> (Xd = %lf, Yd = %lf)\n\n", + Xu, Yu, Xu * Rd / Ru, Yu * Rd / Ru); + } + } else { /* three real roots */ + D = SQRT (-D); + S = CBRT (hypot (R, D)); + T = atan2 (D, R) / 3; + SINCOS (T, sinT, cosT); + + /* the larger positive root is 2*S*cos(T) */ + /* the smaller positive root is -S*cos(T) + SQRT(3)*S*sin(T) */ + /* the negative root is -S*cos(T) - SQRT(3)*S*sin(T) */ + + Rd = -S * cosT + SQRT3 * S * sinT; /* use the smaller positive root */ + } + + lambda = Rd / Ru; + + *Xd = Xu * lambda; + *Yd = Yu * lambda; +} + + +/************************************************************************/ +void distorted_to_undistorted_sensor_coord (Xd, Yd, Xu, Yu) + double Xd, + Yd, + *Xu, + *Yu; +{ + double distortion_factor; + + /* convert from distorted to undistorted sensor plane coordinates */ + distortion_factor = 1 + tsai_cc.kappa1 * (SQR (Xd) + SQR (Yd)); + *Xu = Xd * distortion_factor; + *Yu = Yd * distortion_factor; +} + + +/************************************************************************/ +void undistorted_to_distorted_image_coord (Xfu, Yfu, Xfd, Yfd) + double Xfu, + Yfu, + *Xfd, + *Yfd; +{ + double Xu, + Yu, + Xd, + Yd; + + /* convert from image to sensor coordinates */ + Xu = tsai_cp.dpx * (Xfu - tsai_cp.Cx) / tsai_cp.sx; + Yu = tsai_cp.dpy * (Yfu - tsai_cp.Cy); + + /* convert from undistorted sensor to distorted sensor plane coordinates */ + undistorted_to_distorted_sensor_coord (Xu, Yu, &Xd, &Yd); + + /* convert from sensor to image coordinates */ + *Xfd = Xd * tsai_cp.sx / tsai_cp.dpx + tsai_cp.Cx; + *Yfd = Yd / tsai_cp.dpy + tsai_cp.Cy; +} + + +/************************************************************************/ +void distorted_to_undistorted_image_coord (Xfd, Yfd, Xfu, Yfu) + double Xfd, + Yfd, + *Xfu, + *Yfu; +{ + double Xd, + Yd, + Xu, + Yu; + + /* convert from image to sensor coordinates */ + Xd = tsai_cp.dpx * (Xfd - tsai_cp.Cx) / tsai_cp.sx; + Yd = tsai_cp.dpy * (Yfd - tsai_cp.Cy); + + /* convert from distorted sensor to undistorted sensor plane coordinates */ + distorted_to_undistorted_sensor_coord (Xd, Yd, &Xu, &Yu); + + /* convert from sensor to image coordinates */ + *Xfu = Xu * tsai_cp.sx / tsai_cp.dpx + tsai_cp.Cx; + *Yfu = Yu / tsai_cp.dpy + tsai_cp.Cy; +} + + +/***********************************************************************\ +* This routine takes the position of a point in world coordinates [mm] * +* and determines the position of its image in image coordinates [pix]. * +\***********************************************************************/ +void world_coord_to_image_coord (xw, yw, zw, Xf, Yf) + double xw, + yw, + zw, + *Xf, + *Yf; +{ + double xc, + yc, + zc, + Xu, + Yu, + Xd, + Yd; + + /* convert from world coordinates to camera coordinates */ + xc = tsai_cc.r1 * xw + tsai_cc.r2 * yw + tsai_cc.r3 * zw + tsai_cc.Tx; + yc = tsai_cc.r4 * xw + tsai_cc.r5 * yw + tsai_cc.r6 * zw + tsai_cc.Ty; + zc = tsai_cc.r7 * xw + tsai_cc.r8 * yw + tsai_cc.r9 * zw + tsai_cc.Tz; + + /* convert from camera coordinates to undistorted sensor plane coordinates */ + Xu = tsai_cc.f * xc / zc; + Yu = tsai_cc.f * yc / zc; + + /* convert from undistorted to distorted sensor plane coordinates */ + undistorted_to_distorted_sensor_coord (Xu, Yu, &Xd, &Yd); + + /* convert from distorted sensor plane coordinates to image coordinates */ + *Xf = Xd * tsai_cp.sx / tsai_cp.dpx + tsai_cp.Cx; + *Yf = Yd / tsai_cp.dpy + tsai_cp.Cy; +} + + +/***********************************************************************\ +* This routine performs an inverse perspective projection to determine * +* the position of a point in world coordinates that corresponds to a * +* given position in image coordinates. To constrain the inverse * +* projection to a single point the routine requires a Z world * +* coordinate for the point in addition to the X and Y image coordinates.* +\***********************************************************************/ +void image_coord_to_world_coord (Xfd, Yfd, zw, xw, yw) + double Xfd, + Yfd, + zw, + *xw, + *yw; +{ + double Xd, + Yd, + Xu, + Yu, + common_denominator; + + /* convert from image to distorted sensor coordinates */ + Xd = tsai_cp.dpx * (Xfd - tsai_cp.Cx) / tsai_cp.sx; + Yd = tsai_cp.dpy * (Yfd - tsai_cp.Cy); + + /* convert from distorted sensor to undistorted sensor plane coordinates */ + distorted_to_undistorted_sensor_coord (Xd, Yd, &Xu, &Yu); + + /* calculate the corresponding xw and yw world coordinates */ + /* (these equations were derived by simply inverting */ + /* the perspective projection equations using Macsyma) */ + common_denominator = ((tsai_cc.r1 * tsai_cc.r8 - tsai_cc.r2 * tsai_cc.r7) * Yu + + (tsai_cc.r5 * tsai_cc.r7 - tsai_cc.r4 * tsai_cc.r8) * Xu - + tsai_cc.f * tsai_cc.r1 * tsai_cc.r5 + tsai_cc.f * tsai_cc.r2 * tsai_cc.r4); + + *xw = (((tsai_cc.r2 * tsai_cc.r9 - tsai_cc.r3 * tsai_cc.r8) * Yu + + (tsai_cc.r6 * tsai_cc.r8 - tsai_cc.r5 * tsai_cc.r9) * Xu - + tsai_cc.f * tsai_cc.r2 * tsai_cc.r6 + tsai_cc.f * tsai_cc.r3 * tsai_cc.r5) * zw + + (tsai_cc.r2 * tsai_cc.Tz - tsai_cc.r8 * tsai_cc.Tx) * Yu + + (tsai_cc.r8 * tsai_cc.Ty - tsai_cc.r5 * tsai_cc.Tz) * Xu - + tsai_cc.f * tsai_cc.r2 * tsai_cc.Ty + tsai_cc.f * tsai_cc.r5 * tsai_cc.Tx) / common_denominator; + + *yw = -(((tsai_cc.r1 * tsai_cc.r9 - tsai_cc.r3 * tsai_cc.r7) * Yu + + (tsai_cc.r6 * tsai_cc.r7 - tsai_cc.r4 * tsai_cc.r9) * Xu - + tsai_cc.f * tsai_cc.r1 * tsai_cc.r6 + tsai_cc.f * tsai_cc.r3 * tsai_cc.r4) * zw + + (tsai_cc.r1 * tsai_cc.Tz - tsai_cc.r7 * tsai_cc.Tx) * Yu + + (tsai_cc.r7 * tsai_cc.Ty - tsai_cc.r4 * tsai_cc.Tz) * Xu - + tsai_cc.f * tsai_cc.r1 * tsai_cc.Ty + tsai_cc.f * tsai_cc.r4 * tsai_cc.Tx) / common_denominator; +} + + +/***********************************************************************\ +* This routine takes the position of a point in world coordinates [mm] * +* and determines its position in camera coordinates [mm]. * +\***********************************************************************/ +void world_coord_to_camera_coord (xw, yw, zw, xc, yc, zc) + double xw, + yw, + zw, + *xc, + *yc, + *zc; +{ + *xc = tsai_cc.r1 * xw + tsai_cc.r2 * yw + tsai_cc.r3 * zw + tsai_cc.Tx; + *yc = tsai_cc.r4 * xw + tsai_cc.r5 * yw + tsai_cc.r6 * zw + tsai_cc.Ty; + *zc = tsai_cc.r7 * xw + tsai_cc.r8 * yw + tsai_cc.r9 * zw + tsai_cc.Tz; +} + + +/***********************************************************************\ +* This routine takes the position of a point in camera coordinates [mm] * +* and determines its position in world coordinates [mm]. * +\***********************************************************************/ +void camera_coord_to_world_coord (xc, yc, zc, xw, yw, zw) + double xc, + yc, + zc, + *xw, + *yw, + *zw; +{ + double common_denominator; + + /* these equations were found by simply inverting the previous routine using Macsyma */ + + common_denominator = ((tsai_cc.r1 * tsai_cc.r5 - tsai_cc.r2 * tsai_cc.r4) * tsai_cc.r9 + + (tsai_cc.r3 * tsai_cc.r4 - tsai_cc.r1 * tsai_cc.r6) * tsai_cc.r8 + + (tsai_cc.r2 * tsai_cc.r6 - tsai_cc.r3 * tsai_cc.r5) * tsai_cc.r7); + + *xw = ((tsai_cc.r2 * tsai_cc.r6 - tsai_cc.r3 * tsai_cc.r5) * zc + + (tsai_cc.r3 * tsai_cc.r8 - tsai_cc.r2 * tsai_cc.r9) * yc + + (tsai_cc.r5 * tsai_cc.r9 - tsai_cc.r6 * tsai_cc.r8) * xc + + (tsai_cc.r3 * tsai_cc.r5 - tsai_cc.r2 * tsai_cc.r6) * tsai_cc.Tz + + (tsai_cc.r2 * tsai_cc.r9 - tsai_cc.r3 * tsai_cc.r8) * tsai_cc.Ty + + (tsai_cc.r6 * tsai_cc.r8 - tsai_cc.r5 * tsai_cc.r9) * tsai_cc.Tx) / common_denominator; + + *yw = -((tsai_cc.r1 * tsai_cc.r6 - tsai_cc.r3 * tsai_cc.r4) * zc + + (tsai_cc.r3 * tsai_cc.r7 - tsai_cc.r1 * tsai_cc.r9) * yc + + (tsai_cc.r4 * tsai_cc.r9 - tsai_cc.r6 * tsai_cc.r7) * xc + + (tsai_cc.r3 * tsai_cc.r4 - tsai_cc.r1 * tsai_cc.r6) * tsai_cc.Tz + + (tsai_cc.r1 * tsai_cc.r9 - tsai_cc.r3 * tsai_cc.r7) * tsai_cc.Ty + + (tsai_cc.r6 * tsai_cc.r7 - tsai_cc.r4 * tsai_cc.r9) * tsai_cc.Tx) / common_denominator; + + *zw = ((tsai_cc.r1 * tsai_cc.r5 - tsai_cc.r2 * tsai_cc.r4) * zc + + (tsai_cc.r2 * tsai_cc.r7 - tsai_cc.r1 * tsai_cc.r8) * yc + + (tsai_cc.r4 * tsai_cc.r8 - tsai_cc.r5 * tsai_cc.r7) * xc + + (tsai_cc.r2 * tsai_cc.r4 - tsai_cc.r1 * tsai_cc.r5) * tsai_cc.Tz + + (tsai_cc.r1 * tsai_cc.r8 - tsai_cc.r2 * tsai_cc.r7) * tsai_cc.Ty + + (tsai_cc.r5 * tsai_cc.r7 - tsai_cc.r4 * tsai_cc.r8) * tsai_cc.Tx) / common_denominator; +} diff --git a/others/pyTsai/src/tsai/cal_util.c b/others/pyTsai/src/tsai/cal_util.c new file mode 100644 index 0000000..61fa6b8 --- /dev/null +++ b/others/pyTsai/src/tsai/cal_util.c @@ -0,0 +1,227 @@ +/** + * cal_util.c + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/****************************************************************************\ +* * +* This file contains utility routines for dumping, loading and printing * +* the data structures used by the routines contained in the file * +* cal_main.c. * +* * +* History * +* ------- * +* * +* 01-Apr-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN * +* Filename changes for DOS port. * +* * +* 04-Jun-94 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN * +* Added alternate macro definitions for less common math functions. * +* * +* 30-Nov-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * +* Updated to use new calibration statistics routines. * +* * +* 01-May-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * +* Original implementation. * +* * +* * +\****************************************************************************/ + +/** This code is not required for pytsai. */ +#if 0 + +#include +#include +#include "cal_main.h" + +#define PI 3.14159265358979323846264338327950288419716939937511 + + +void load_cd_data (fp, cd) + FILE *fp; + struct calibration_data *cd; +{ + cd->point_count = 0; + while (fscanf (fp, "%lf %lf %lf %lf %lf", + &(cd->xw[cd->point_count]), + &(cd->yw[cd->point_count]), + &(cd->zw[cd->point_count]), + &(cd->Xf[cd->point_count]), + &(cd->Yf[cd->point_count])) != EOF) + if (cd->point_count++ >= MAX_POINTS) { + fprintf (stderr, "load_cd_data: too many points, compiled in limit is %d\n", MAX_POINTS); + exit (-1); + } +} + + +void dump_cd_data (fp, cd) + FILE *fp; + struct calibration_data *cd; +{ + int i; + + for (i = 0; i < cd->point_count; i++) + fprintf (fp, "%17.10le %17.10le %17.10le %17.10le %17.10le\n", + cd->xw[i], + cd->yw[i], + cd->zw[i], + cd->Xf[i], + cd->Yf[i]); +} + + +void load_cp_cc_data (fp, cp, cc) + FILE *fp; + struct camera_parameters *cp; + struct calibration_constants *cc; +{ + double sa, + ca, + sb, + cb, + sg, + cg; + + fscanf (fp, "%lf", &(cp->Ncx)); + fscanf (fp, "%lf", &(cp->Nfx)); + fscanf (fp, "%lf", &(cp->dx)); + fscanf (fp, "%lf", &(cp->dy)); + fscanf (fp, "%lf", &(cp->dpx)); + fscanf (fp, "%lf", &(cp->dpy)); + fscanf (fp, "%lf", &(cp->Cx)); + fscanf (fp, "%lf", &(cp->Cy)); + fscanf (fp, "%lf", &(cp->sx)); + + fscanf (fp, "%lf", &(cc->f)); + fscanf (fp, "%lf", &(cc->kappa1)); + fscanf (fp, "%lf", &(cc->Tx)); + fscanf (fp, "%lf", &(cc->Ty)); + fscanf (fp, "%lf", &(cc->Tz)); + fscanf (fp, "%lf", &(cc->Rx)); + fscanf (fp, "%lf", &(cc->Ry)); + fscanf (fp, "%lf", &(cc->Rz)); + + SINCOS (cc->Rx, sa, ca); + SINCOS (cc->Ry, sb, cb); + SINCOS (cc->Rz, sg, cg); + + cc->r1 = cb * cg; + cc->r2 = cg * sa * sb - ca * sg; + cc->r3 = sa * sg + ca * cg * sb; + cc->r4 = cb * sg; + cc->r5 = sa * sb * sg + ca * cg; + cc->r6 = ca * sb * sg - cg * sa; + cc->r7 = -sb; + cc->r8 = cb * sa; + cc->r9 = ca * cb; + + fscanf (fp, "%lf", &(cc->p1)); + fscanf (fp, "%lf", &(cc->p2)); +} + + +void dump_cp_cc_data (fp, cp, cc) + FILE *fp; + struct camera_parameters *cp; + struct calibration_constants *cc; +{ + fprintf (fp, "%17.10le\n", cp->Ncx); + fprintf (fp, "%17.10le\n", cp->Nfx); + fprintf (fp, "%17.10le\n", cp->dx); + fprintf (fp, "%17.10le\n", cp->dy); + fprintf (fp, "%17.10le\n", cp->dpx); + fprintf (fp, "%17.10le\n", cp->dpy); + fprintf (fp, "%17.10le\n", cp->Cx); + fprintf (fp, "%17.10le\n", cp->Cy); + fprintf (fp, "%17.10le\n", cp->sx); + + fprintf (fp, "%17.10le\n", cc->f); + fprintf (fp, "%17.10le\n", cc->kappa1); + fprintf (fp, "%17.10le\n", cc->Tx); + fprintf (fp, "%17.10le\n", cc->Ty); + fprintf (fp, "%17.10le\n", cc->Tz); + fprintf (fp, "%17.10le\n", cc->Rx); + fprintf (fp, "%17.10le\n", cc->Ry); + fprintf (fp, "%17.10le\n", cc->Rz); + fprintf (fp, "%17.10le\n", cc->p1); + fprintf (fp, "%17.10le\n", cc->p2); +} + + +void print_cp_cc_data (fp, cp, cc) + FILE *fp; + struct camera_parameters *cp; + struct calibration_constants *cc; +{ + fprintf (fp, " f = %.6lf [mm]\n\n", cc->f); + + fprintf (fp, " kappa1 = %.6le [1/mm^2]\n\n", cc->kappa1); + + fprintf (fp, " Tx = %.6lf, Ty = %.6lf, Tz = %.6lf [mm]\n\n", cc->Tx, cc->Ty, cc->Tz); + + fprintf (fp, " Rx = %.6lf, Ry = %.6lf, Rz = %.6lf [deg]\n\n", + cc->Rx * 180 / PI, cc->Ry * 180 / PI, cc->Rz * 180 / PI); + + fprintf (fp, " R\n"); + fprintf (fp, " %9.6lf %9.6lf %9.6lf\n", cc->r1, cc->r2, cc->r3); + fprintf (fp, " %9.6lf %9.6lf %9.6lf\n", cc->r4, cc->r5, cc->r6); + fprintf (fp, " %9.6lf %9.6lf %9.6lf\n\n", cc->r7, cc->r8, cc->r9); + + fprintf (fp, " sx = %.6lf\n", cp->sx); + fprintf (fp, " Cx = %.6lf, Cy = %.6lf [pixels]\n\n", cp->Cx, cp->Cy); + + fprintf (fp, " Tz / f = %.6lf\n\n", cc->Tz / cc->f); +} + + +void print_error_stats (fp) + FILE *fp; +{ + double mean, + stddev, + max, + sse; + + /* calculate the distorted image plane error statistics for the data set */ + distorted_image_plane_error_stats (&(mean), &(stddev), &(max), &(sse)); + fprintf (fp, " distorted image plane error:\n"); + fprintf (fp, + " mean = %.6lf, stddev = %.6lf, max = %.6lf [pix], sse = %.6lf [pix^2]\n\n", + mean, stddev, max, sse); + + /* calculate the undistorted image plane error statistics for the data set */ + undistorted_image_plane_error_stats (&(mean), &(stddev), &(max), &(sse)); + fprintf (fp, " undistorted image plane error:\n"); + fprintf (fp, + " mean = %.6lf, stddev = %.6lf, max = %.6lf [pix], sse = %.6lf [pix^2]\n\n", + mean, stddev, max, sse); + + /* calculate the undistorted image plane error statistics for the data set */ + object_space_error_stats (&(mean), &(stddev), &(max), &(sse)); + fprintf (fp, " object space error:\n"); + fprintf (fp, + " mean = %.6lf, stddev = %.6lf, max = %.6lf [mm], sse = %.6lf [mm^2]\n\n", + mean, stddev, max, sse); + + /* calculate the error statistics for the data set */ + normalized_calibration_error (&(mean), &(stddev)); + + fprintf (fp, " normalized calibration error: %.6lf\n\n", mean); +} + +#endif diff --git a/others/pyTsai/src/tsai/cal_util.h b/others/pyTsai/src/tsai/cal_util.h new file mode 100644 index 0000000..0842d1a --- /dev/null +++ b/others/pyTsai/src/tsai/cal_util.h @@ -0,0 +1,46 @@ +/** + * cal_util.h + * Copyright (C) Jonathan Merritt 2004. + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/** + * Forward declaration of routines in the cal_util.c file. + */ + +/** This code is not required for pytsai. */ +#if 0 + +#ifndef CAL_UTIL_H +#define CAL_UTIL_H + +#include +#include "cal_main.h" + +void load_cd_data(FILE *fp, struct calibration_data *cd); +void dump_cd_data(FILE *fp, struct calibration_data *cd); +void load_cp_cc_data(FILE *fp, struct camera_parameters *cp, + struct calibration_constants *cc); +void dump_cp_cc_data(FILE *fp, struct camera_parameters *cp, + struct calibration_constants *cc); +void print_cp_cc_data(FILE *fp, struct camera_parameters *cp, + struct calibration_constants *cc); +void print_error_stats(FILE *fp); + +#endif /* CAL_UTIL_H */ + +#endif diff --git a/others/pyTsai/src/tsai/ecalmain.c b/others/pyTsai/src/tsai/ecalmain.c new file mode 100644 index 0000000..aba3c85 --- /dev/null +++ b/others/pyTsai/src/tsai/ecalmain.c @@ -0,0 +1,890 @@ +/** + * ecalmain.c + * + * This library is free software; you can redistribute it and/or + * modify it under the terms of the GNU Lesser General Public + * License as published by the Free Software Foundation; either + * version 2 of the License, or (at your option) any later version. + * + * This library is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Library General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with this library; if not, write to the + * Free Software Foundation, Inc., 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +/****************************************************************************\ +* * +* This file contains routines for calibrating the extrinsic parameters of * +* Tsai's 11 parameter camera model. The inputs to the routines are a set of * +* precalibrated intrinsic camera parameters: * +* * +* f - effective focal length of the pin hole camera * +* kappa1 - 1st order radial lens distortion coefficient * +* Cx, Cy - coordinates of center of radial lens distortion * +* (also used as the piercing point of the camera coordinate * +* frame's Z axis with the camera's sensor plane) * +* sx - uncertainty factor for scale of horizontal scanline * +* * +* and a set of calibration data consisting of the (x,y,z) world coordinates * +* of a feature point (in mm) and the corresponding coordinates (Xf,Yf) (in * +* pixels) of the feature point in the image. The outputs of the routines * +* are the 6 external (also called extrinsic or exterior) camera parameters: * +* * +* Rx, Ry, Rz, Tx, Ty, Tz - rotational and translational components of * +* the transform between the world's * +* coordinate frame and the camera's * +* coordinate frame. * +* * +* describing the camera's pose. * +* * +* This file provides two routines: * +* * +* coplanar_extrinsic_parameter_estimation () * +* and * +* noncoplanar_extrinsic_parameter_estimation () * +* * +* which are used respectively for coplanar and non-coplanar calibration * +* data. * +* * +* Initial estimates for the extrinsic camera parameters are determined using * +* a modification of the first stage of Tsai's algorithm. These estimates * +* are then refined using iterative non-linear optimization. * +* * +* * +* History * +* ------- * +* * +* 01-Nov-04 Jonathan Merritt (j.merritt@pgrad.unimelb.edu.au) at * +* The University of Melbourne. * +* o Released new changes under the LGPL. * +* o Removed exit() calls. * +* * +* 15-Oct-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN * +* Added routines to coplanar_extrinsic_parameter_estimation to pick * +* the correct rotation matrix solution from the two possible solutions.* +* Bug tracked down by Pete Rander . * +* * +* 20-May-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN * +* Return the error to lmdif rather than the squared error. * +* lmdif calculates the squared error internally during optimization. * +* Before this change calibration was essentially optimizing error^4. * +* * +* 02-Apr-95 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN * +* Rewrite memory allocation to avoid memory alignment problems * +* on some machines. * +* Strip out IMSL code. MINPACK seems to work fine. * +* Filename changes for DOS port. * +* * +* 04-Jun-94 Reg Willson (rgwillson@mmm.com) at 3M St. Paul, MN * +* Added alternate macro definitions for less common math functions. * +* * +* 25-Mar-94 Torfi Thorhallsson (torfit@verk.hi.is) at the University of * +* Iceland * +* Added a new version of the routine epe_optimize() which uses the * +* *public domain* MINPACK optimization library instead of IMSL. * +* To select the new routine, compile this file with the flag -DMINPACK * +* * +* 11-Nov-93 Reg Willson (rgw@cs.cmu.edu) at Carnegie-Mellon University * +* Original implementation. * +* * +\****************************************************************************/ + +#include +#include +#include +#include +#include "../matrix/matrix.h" +#include "cal_main.h" +#include "../minpack/f2c.h" +#include "../minpack/minpack.h" +#include "../errors.h" + + +/***********************************************************************\ +* Routines for coplanar extrinsic parameter estimation * +\***********************************************************************/ +/* pytsai: can fail; int return type is required. */ +int cepe_compute_U (U) + double U[]; +{ + dmat M, + a, + b; + + double Xd, + Yd, + Xu, + Yu, + distortion_factor; + + int i; + + M = newdmat (0, (tsai_cd.point_count - 1), 0, 4, &errno); + if (errno) { + pytsai_raise("cepe compute U: unable to allocate matrix M"); + return 0; + } + + a = newdmat (0, 4, 0, 0, &errno); + if (errno) { + freemat(M); + pytsai_raise("cepe compute U: unable to allocate vector a"); + return 0; + } + + b = newdmat (0, (tsai_cd.point_count - 1), 0, 0, &errno); + if (errno) { + freemat(M); + freemat(a); + pytsai_raise("cepe compute U: unable to allocate vector b"); + return 0; + } + + for (i = 0; i < tsai_cd.point_count; i++) { + /* convert from image coordinates to distorted sensor coordinates */ + Xd = tsai_cp.dpx * (tsai_cd.Xf[i] - tsai_cp.Cx) / tsai_cp.sx; + Yd = tsai_cp.dpy * (tsai_cd.Yf[i] - tsai_cp.Cy); + + /* convert from distorted sensor coordinates to undistorted sensor coordinates */ + distortion_factor = 1 + tsai_cc.kappa1 * (SQR (Xd) + SQR (Yd)); + Xu = Xd * distortion_factor; + Yu = Yd * distortion_factor; + + M.el[i][0] = Yu * tsai_cd.xw[i]; + M.el[i][1] = Yu * tsai_cd.yw[i]; + M.el[i][2] = Yu; + M.el[i][3] = -Xu * tsai_cd.xw[i]; + M.el[i][4] = -Xu * tsai_cd.yw[i]; + b.el[i][0] = Xu; + } + + if (solve_system (M, a, b)) { + freemat(M); + freemat(a); + freemat(b); + pytsai_raise("cepe compute U: unable to solve system Ma=b"); + return 0; + } + + U[0] = a.el[0][0]; + U[1] = a.el[1][0]; + U[2] = a.el[2][0]; + U[3] = a.el[3][0]; + U[4] = a.el[4][0]; + + freemat (M); + freemat (a); + freemat (b); + + return 1; +} + + +/* pytsai: cannot fail; void return type is fine. */ +void cepe_compute_Tx_and_Ty (U) + double U[]; +{ + double Tx, + Ty, + Ty_squared, + x, + y, + Sr, + r1p, + r2p, + r4p, + r5p, + r1, + r2, + r4, + r5, + distance, + far_distance; + + int i, + far_point; + + r1p = U[0]; + r2p = U[1]; + r4p = U[3]; + r5p = U[4]; + + /* first find the square of the magnitude of Ty */ + if ((fabs (r1p) < EPSILON) && (fabs (r2p) < EPSILON)) + Ty_squared = 1 / (SQR (r4p) + SQR (r5p)); + else if ((fabs (r4p) < EPSILON) && (fabs (r5p) < EPSILON)) + Ty_squared = 1 / (SQR (r1p) + SQR (r2p)); + else if ((fabs (r1p) < EPSILON) && (fabs (r4p) < EPSILON)) + Ty_squared = 1 / (SQR (r2p) + SQR (r5p)); + else if ((fabs (r2p) < EPSILON) && (fabs (r5p) < EPSILON)) + Ty_squared = 1 / (SQR (r1p) + SQR (r4p)); + else { + Sr = SQR (r1p) + SQR (r2p) + SQR (r4p) + SQR (r5p); + Ty_squared = (Sr - sqrt (SQR (Sr) - 4 * SQR (r1p * r5p - r4p * r2p))) / + (2 * SQR (r1p * r5p - r4p * r2p)); + } + + /* find a point that is far from the image center */ + far_distance = 0; + far_point = 0; + for (i = 0; i < tsai_cd.point_count; i++) + if ((distance = SQR (tsai_cd.Xf[i] - tsai_cp.Cx) + SQR (tsai_cd.Yf[i] - tsai_cp.Cy)) > far_distance) { + far_point = i; + far_distance = distance; + } + + /* now find the sign for Ty */ + /* start by assuming Ty > 0 */ + Ty = sqrt (Ty_squared); + r1 = U[0] * Ty; + r2 = U[1] * Ty; + Tx = U[2] * Ty; + r4 = U[3] * Ty; + r5 = U[4] * Ty; + x = r1 * tsai_cd.xw[far_point] + r2 * tsai_cd.yw[far_point] + Tx; + y = r4 * tsai_cd.xw[far_point] + r5 * tsai_cd.yw[far_point] + Ty; + + /* flip Ty if we guessed wrong */ + if ((SIGNBIT (x) != SIGNBIT (tsai_cd.Xf[far_point] - tsai_cp.Cx)) || + (SIGNBIT (y) != SIGNBIT (tsai_cd.Yf[far_point] - tsai_cp.Cy))) + Ty = -Ty; + + /* update the calibration constants */ + tsai_cc.Tx = U[2] * Ty; + tsai_cc.Ty = Ty; +} + + +/* pytsai: cannot fail; void return type is fine. */ +void cepe_compute_R (U) + double U[]; +{ + double r1, + r2, + r3, + r4, + r5, + r6, + r7, + r8, + r9; + + r1 = U[0] * tsai_cc.Ty; + r2 = U[1] * tsai_cc.Ty; + r3 = sqrt (1 - SQR (r1) - SQR (r2)); + + r4 = U[3] * tsai_cc.Ty; + r5 = U[4] * tsai_cc.Ty; + r6 = sqrt (1 - SQR (r4) - SQR (r5)); + if (!SIGNBIT (r1 * r4 + r2 * r5)) + r6 = -r6; + + /* use the outer product of the first two rows to get the last row */ + r7 = r2 * r6 - r3 * r5; + r8 = r3 * r4 - r1 * r6; + r9 = r1 * r5 - r2 * r4; + + /* update the calibration constants */ + tsai_cc.r1 = r1; + tsai_cc.r2 = r2; + tsai_cc.r3 = r3; + tsai_cc.r4 = r4; + tsai_cc.r5 = r5; + tsai_cc.r6 = r6; + tsai_cc.r7 = r7; + tsai_cc.r8 = r8; + tsai_cc.r9 = r9; + + /* fill in tsai_cc.Rx, tsai_cc.Ry and tsai_cc.Rz */ + solve_RPY_transform (); +} + + +/* pytsai: can fail; int return type is required. */ +int cepe_compute_approximate_f (f) + double *f; +{ + dmat M, + a, + b; + + double Yd; + + int i; + + M = newdmat (0, (tsai_cd.point_count - 1), 0, 1, &errno); + if (errno) { + pytsai_raise("cepe compute apx: unable to allocate matrix M"); + return 0; + } + + a = newdmat (0, 1, 0, 0, &errno); + if (errno) { + freemat(M); + pytsai_raise("cepe compute apx: unable to allocate vector a"); + return 0; + } + + b = newdmat (0, (tsai_cd.point_count - 1), 0, 0, &errno); + if (errno) { + freemat(M); + freemat(a); + pytsai_raise("cepe compute apx: unable to allocate vector b"); + return 0; + } + + for (i = 0; i < tsai_cd.point_count; i++) { + Yd = tsai_cp.dpy * (tsai_cd.Yf[i] - tsai_cp.Cy); + + M.el[i][0] = tsai_cc.r4 * tsai_cd.xw[i] + tsai_cc.r5 * tsai_cd.yw[i] + tsai_cc.Ty; + M.el[i][1] = -Yd; + b.el[i][0] = (tsai_cc.r7 * tsai_cd.xw[i] + tsai_cc.r8 * tsai_cd.yw[i]) * Yd; + } + + if (solve_system (M, a, b)) { + freemat(M); + freemat(a); + freemat(b); + pytsai_raise("cepe compute apx: unable to solve system Ma=b"); + return 0; + } + + /* return the approximate effective focal length */ + *f = a.el[0][0]; + + freemat (M); + freemat (a); + freemat (b); + + return 1; +} + + +/***********************************************************************\ +* Routines for noncoplanar extrinsic parameter estimation * +\***********************************************************************/ +/* pytsai: can fail; int return type is required. */ +int ncepe_compute_U (U) + double U[]; +{ + dmat M, + a, + b; + + double Xu, + Yu, + Xd, + Yd, + distortion_factor; + + int i; + + M = newdmat (0, (tsai_cd.point_count - 1), 0, 6, &errno); + if (errno) { + pytsai_raise("ncepe compute U: unable to allocate matrix M"); + return 0; + } + + a = newdmat (0, 6, 0, 0, &errno); + if (errno) { + freemat(M); + pytsai_raise("ncepe compute U: unable to allocate vector a"); + return 0; + } + + b = newdmat (0, (tsai_cd.point_count - 1), 0, 0, &errno); + if (errno) { + freemat(M); + freemat(a); + pytsai_raise("ncepe compute U: unable to allocate vector b"); + return 0; + } + + for (i = 0; i < tsai_cd.point_count; i++) { + /* convert from image coordinates to distorted sensor coordinates */ + Xd = tsai_cp.dpx * (tsai_cd.Xf[i] - tsai_cp.Cx) / tsai_cp.sx; + Yd = tsai_cp.dpy * (tsai_cd.Yf[i] - tsai_cp.Cy); + + /* convert from distorted sensor coordinates to undistorted sensor coordinates */ + distortion_factor = 1 + tsai_cc.kappa1 * (SQR (Xd) + SQR (Yd)); + Xu = Xd * distortion_factor; + Yu = Yd * distortion_factor; + + M.el[i][0] = Yu * tsai_cd.xw[i]; + M.el[i][1] = Yu * tsai_cd.yw[i]; + M.el[i][2] = Yu * tsai_cd.zw[i]; + M.el[i][3] = Yu; + M.el[i][4] = -Xu * tsai_cd.xw[i]; + M.el[i][5] = -Xu * tsai_cd.yw[i]; + M.el[i][6] = -Xu * tsai_cd.zw[i]; + b.el[i][0] = Xu; + } + + if (solve_system (M, a, b)) { + freemat(M); + freemat(a); + freemat(b); + pytsai_raise("ncepe compute U: unable to solve system Ma=b"); + return 0; + } + + U[0] = a.el[0][0]; + U[1] = a.el[1][0]; + U[2] = a.el[2][0]; + U[3] = a.el[3][0]; + U[4] = a.el[4][0]; + U[5] = a.el[5][0]; + U[6] = a.el[6][0]; + + freemat (M); + freemat (a); + freemat (b); + + return 1; +} + + +/* pytsai: cannot fail; void return type is fine. */ +void ncepe_compute_Tx_and_Ty (U) + double U[]; +{ + double Tx, + Ty, + Ty_squared, + x, + y, + r1, + r2, + r3, + r4, + r5, + r6, + distance, + far_distance; + + int i, + far_point; + + /* first find the square of the magnitude of Ty */ + Ty_squared = 1 / (SQR (U[4]) + SQR (U[5]) + SQR (U[6])); + + /* find a point that is far from the image center */ + far_distance = 0; + far_point = 0; + for (i = 0; i < tsai_cd.point_count; i++) + if ((distance = SQR (tsai_cd.Xf[i] - tsai_cp.Cx) + SQR (tsai_cd.Yf[i] - tsai_cp.Cy)) > far_distance) { + far_point = i; + far_distance = distance; + } + + /* now find the sign for Ty */ + /* start by assuming Ty > 0 */ + Ty = sqrt (Ty_squared); + r1 = U[0] * Ty; + r2 = U[1] * Ty; + r3 = U[2] * Ty; + Tx = U[3] * Ty; + r4 = U[4] * Ty; + r5 = U[5] * Ty; + r6 = U[6] * Ty; + x = r1 * tsai_cd.xw[far_point] + r2 * tsai_cd.yw[far_point] + r3 * tsai_cd.zw[far_point] + Tx; + y = r4 * tsai_cd.xw[far_point] + r5 * tsai_cd.yw[far_point] + r6 * tsai_cd.zw[far_point] + Ty; + + /* flip Ty if we guessed wrong */ + if ((SIGNBIT (x) != SIGNBIT (tsai_cd.Xf[far_point] - tsai_cp.Cx)) || + (SIGNBIT (y) != SIGNBIT (tsai_cd.Yf[far_point] - tsai_cp.Cy))) + Ty = -Ty; + + /* update the calibration constants */ + tsai_cc.Tx = U[3] * Ty; + tsai_cc.Ty = Ty; +} + + +/* pytsai: cannot fail; void return type is fine. */ +void ncepe_compute_R (U) + double U[]; +{ + double r1, + r2, + r3, + r4, + r5, + r6, + r7, + r8, + r9; + + r1 = U[0] * tsai_cc.Ty; + r2 = U[1] * tsai_cc.Ty; + r3 = U[2] * tsai_cc.Ty; + + r4 = U[4] * tsai_cc.Ty; + r5 = U[5] * tsai_cc.Ty; + r6 = U[6] * tsai_cc.Ty; + + /* use the outer product of the first two rows to get the last row */ + r7 = r2 * r6 - r3 * r5; + r8 = r3 * r4 - r1 * r6; + r9 = r1 * r5 - r2 * r4; + + /* update the calibration constants */ + tsai_cc.r1 = r1; + tsai_cc.r2 = r2; + tsai_cc.r3 = r3; + tsai_cc.r4 = r4; + tsai_cc.r5 = r5; + tsai_cc.r6 = r6; + tsai_cc.r7 = r7; + tsai_cc.r8 = r8; + tsai_cc.r9 = r9; + + /* fill in tsai_cc.Rx, tsai_cc.Ry and tsai_cc.Rz */ + solve_RPY_transform (); +} + + +/************************************************************************/ +/* pytsai: can fail; int return type is required. */ +int epe_compute_Tx_Ty_Tz () +{ + dmat M, + a, + b; + + double xk, + yk, + zk, + Xu, + Yu, + Xd, + Yd, + distortion_factor; + + int i, + j; + + M = newdmat (0, (2 * tsai_cd.point_count - 1), 0, 2, &errno); + if (errno) { + pytsai_raise("epe compute Tx Ty Tz: unable to allocate matrix M"); + return 0; + } + + a = newdmat (0, 2, 0, 0, &errno); + if (errno) { + freemat(M); + pytsai_raise("epe compute Tx Ty Tz: unable to allocate vector a"); + return 0; + } + + b = newdmat (0, (2 * tsai_cd.point_count - 1), 0, 0, &errno); + if (errno) { + freemat(M); + freemat(a); + pytsai_raise("epe compute Tx Ty Tz: unable to allocate vector b"); + return 0; + } + + for (i = 0, j = tsai_cd.point_count; i < tsai_cd.point_count; i++, j++) { + /* convert from world coordinates to untranslated camera coordinates */ + xk = tsai_cc.r1 * tsai_cd.xw[i] + tsai_cc.r2 * tsai_cd.yw[i] + tsai_cc.r3 * tsai_cd.zw[i]; + yk = tsai_cc.r4 * tsai_cd.xw[i] + tsai_cc.r5 * tsai_cd.yw[i] + tsai_cc.r6 * tsai_cd.zw[i]; + zk = tsai_cc.r7 * tsai_cd.xw[i] + tsai_cc.r8 * tsai_cd.yw[i] + tsai_cc.r9 * tsai_cd.zw[i]; + + /* convert from image coordinates to distorted sensor coordinates */ + Xd = tsai_cp.dpx * (tsai_cd.Xf[i] - tsai_cp.Cx) / tsai_cp.sx; + Yd = tsai_cp.dpy * (tsai_cd.Yf[i] - tsai_cp.Cy); + + /* convert from distorted sensor coordinates to undistorted sensor coordinates */ + distortion_factor = 1 + tsai_cc.kappa1 * (SQR (Xd) + SQR (Yd)); + Xu = Xd * distortion_factor; + Yu = Yd * distortion_factor; + + M.el[i][0] = tsai_cc.f; + M.el[i][1] = 0; + M.el[i][2] = -Xu; + b.el[i][0] = Xu * zk - tsai_cc.f * xk; + + M.el[j][0] = 0; + M.el[j][1] = tsai_cc.f; + M.el[j][2] = -Yu; + b.el[j][0] = Yu * zk - tsai_cc.f * yk; + } + + if (solve_system (M, a, b)) { + freemat(M); + freemat(a); + freemat(b); + pytsai_raise("epe compute Tx Ty Tz: unable to solve system Ma=b"); + return 0; + } + + tsai_cc.Tx = a.el[0][0]; + tsai_cc.Ty = a.el[1][0]; + tsai_cc.Tz = a.el[2][0]; + + freemat (M); + freemat (a); + freemat (b); + + return 1; +} + + +/************************************************************************/ +/* pytsai: cannot fail; void return type is fine. */ +void epe_optimize_error (m_ptr, n_ptr, params, err) + integer *m_ptr; /* pointer to number of points to fit */ + integer *n_ptr; /* pointer to number of parameters */ + doublereal *params; /* vector of parameters */ + doublereal *err; /* vector of error from data */ +{ + int i; + + double xc, + yc, + zc, + Xd, + Yd, + Xu_1, + Yu_1, + Xu_2, + Yu_2, + distortion_factor, + Rx, + Ry, + Rz, + Tx, + Ty, + Tz, + r1, + r2, + r3, + r4, + r5, + r6, + r7, + r8, + r9, + sa, + sb, + sg, + ca, + cb, + cg; + + Rx = params[0]; + Ry = params[1]; + Rz = params[2]; + Tx = params[3]; + Ty = params[4]; + Tz = params[5]; + + SINCOS (Rx, sa, ca); + SINCOS (Ry, sb, cb); + SINCOS (Rz, sg, cg); + + r1 = cb * cg; + r2 = cg * sa * sb - ca * sg; + r3 = sa * sg + ca * cg * sb; + r4 = cb * sg; + r5 = sa * sb * sg + ca * cg; + r6 = ca * sb * sg - cg * sa; + r7 = -sb; + r8 = cb * sa; + r9 = ca * cb; + + for (i = 0; i < tsai_cd.point_count; i++) { + /* convert from world coordinates to camera coordinates */ + xc = r1 * tsai_cd.xw[i] + r2 * tsai_cd.yw[i] + r3 * tsai_cd.zw[i] + Tx; + yc = r4 * tsai_cd.xw[i] + r5 * tsai_cd.yw[i] + r6 * tsai_cd.zw[i] + Ty; + zc = r7 * tsai_cd.xw[i] + r8 * tsai_cd.yw[i] + r9 * tsai_cd.zw[i] + Tz; + + /* convert from camera coordinates to undistorted sensor plane coordinates */ + Xu_1 = tsai_cc.f * xc / zc; + Yu_1 = tsai_cc.f * yc / zc; + + /* convert from 2D image coordinates to distorted sensor coordinates */ + Xd = tsai_cp.dpx * (tsai_cd.Xf[i] - tsai_cp.Cx) / tsai_cp.sx; + Yd = tsai_cp.dpy * (tsai_cd.Yf[i] - tsai_cp.Cy); + + /* convert from distorted sensor coordinates to undistorted sensor plane coordinates */ + distortion_factor = 1 + tsai_cc.kappa1 * (SQR (Xd) + SQR (Yd)); + Xu_2 = Xd * distortion_factor; + Yu_2 = Yd * distortion_factor; + + /* record the error in the undistorted sensor coordinates */ + err[i] = hypot (Xu_1 - Xu_2, Yu_1 - Yu_2); + } +} + + +/* pytsai: can fail; int return type is required. */ +int epe_optimize () +{ +#define NPARAMS 6 + + int i; + + /* Parameters needed by MINPACK's lmdif() */ + + integer m = tsai_cd.point_count; + integer n = NPARAMS; + doublereal x[NPARAMS]; + doublereal *fvec; + doublereal ftol = REL_SENSOR_TOLERANCE_ftol; + doublereal xtol = REL_PARAM_TOLERANCE_xtol; + doublereal gtol = ORTHO_TOLERANCE_gtol; + integer maxfev = MAXFEV; + doublereal epsfcn = EPSFCN; + doublereal diag[NPARAMS]; + integer mode = MODE; + doublereal factor = FACTOR; + integer nprint = 0; + integer info; + integer nfev; + doublereal *fjac; + integer ldfjac = m; + integer ipvt[NPARAMS]; + doublereal qtf[NPARAMS]; + doublereal wa1[NPARAMS]; + doublereal wa2[NPARAMS]; + doublereal wa3[NPARAMS]; + doublereal *wa4; + + /* allocate some workspace */ + if (( fvec = (doublereal *) PyMem_Malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + pytsai_raise("PyMem_Malloc: Cannot allocate workspace fvec"); + return 0; + } + + if (( fjac = (doublereal *) PyMem_Malloc ((unsigned int) m*n * (unsigned int) sizeof(doublereal))) == NULL ) { + PyMem_Free(fvec); + pytsai_raise("PyMem_Malloc: Cannot allocate workspace fjac"); + return 0; + } + + if (( wa4 = (doublereal *) PyMem_Malloc ((unsigned int) m * (unsigned int) sizeof(doublereal))) == NULL ) { + PyMem_Free(fvec); + PyMem_Free(fjac); + pytsai_raise("PyMem_Malloc: Cannot allocate workspace wa4"); + return 0; + } + + /* use the current calibration and camera constants as a starting point */ + x[0] = tsai_cc.Rx; + x[1] = tsai_cc.Ry; + x[2] = tsai_cc.Rz; + x[3] = tsai_cc.Tx; + x[4] = tsai_cc.Ty; + x[5] = tsai_cc.Tz; + + /* define optional scale factors for the parameters */ + if ( mode == 2 ) { + for (i = 0; i < NPARAMS; i++) + diag[i] = 1.0; /* some user-defined values */ + } + + /* perform the optimization */ + lmdif_ (epe_optimize_error, + &m, &n, x, fvec, &ftol, &xtol, >ol, &maxfev, &epsfcn, + diag, &mode, &factor, &nprint, &info, &nfev, fjac, &ldfjac, + ipvt, qtf, wa1, wa2, wa3, wa4); + /* TODO: Check for error conditions (into parameter). */ + + /* update the calibration and camera constants */ + tsai_cc.Rx = x[0]; + tsai_cc.Ry = x[1]; + tsai_cc.Rz = x[2]; + apply_RPY_transform (); + + tsai_cc.Tx = x[3]; + tsai_cc.Ty = x[4]; + tsai_cc.Tz = x[5]; + + /* release allocated workspace */ + PyMem_Free(fvec); + PyMem_Free(fjac); + PyMem_Free(wa4); + +#ifdef DEBUG + /* print the number of function calls during iteration */ + fprintf(stderr,"info: %d nfev: %d\n\n",info,nfev); +#endif + + return 1; + +#undef NPARAMS +} + + +/************************************************************************/ +/* pytsai: can fail; int return type is required. */ +int coplanar_extrinsic_parameter_estimation () +{ + double trial_f, + U[5]; + + if (!cepe_compute_U(U)) + return 0; + + cepe_compute_Tx_and_Ty (U); + + cepe_compute_R (U); + + if (!cepe_compute_approximate_f(&trial_f)) + return 0; + + if (trial_f < 0) { + /* try the other rotation matrix solution */ + tsai_cc.r3 = -tsai_cc.r3; + tsai_cc.r6 = -tsai_cc.r6; + tsai_cc.r7 = -tsai_cc.r7; + tsai_cc.r8 = -tsai_cc.r8; + solve_RPY_transform (); + + if (!cepe_compute_approximate_f(&trial_f)) + return 0; + + if (trial_f < 0) { + pytsai_raise("error - possible handedness problem with data"); + return 0; + } + } + + if (!epe_compute_Tx_Ty_Tz()) + return 0; + + if (!epe_optimize()) + return 0; + + return 1; +} + + +/************************************************************************/ +/* pytsai: can fail; int return type is required. */ +int noncoplanar_extrinsic_parameter_estimation () +{ + double U[7]; + + if (!ncepe_compute_U(U)) + return 0; + + ncepe_compute_Tx_and_Ty (U); + + ncepe_compute_R (U); + + if (!epe_compute_Tx_Ty_Tz()) + return 0; + + if (!epe_optimize()) + return 0; + + return 1; +} diff --git a/others/pyTsai/test/TestCoplanar.py b/others/pyTsai/test/TestCoplanar.py new file mode 100644 index 0000000..3da70de --- /dev/null +++ b/others/pyTsai/test/TestCoplanar.py @@ -0,0 +1,21 @@ +#!/usr/bin/env python + +""" +TODO: Finish. +""" + +def grid(side, n): + """ + Returns a grid with specified side length and number of points, + centred at the origin and with z=0. + """ + if n < 2: + raise TypeError('n must be >= 2') + coords = [] + for yi in range(0,n): + y = float(yi) / (n-1) * side - (side/2.0) + for xi in range(0,n): + x = float(xi) / (n-1) * side - (side/2.0) + coords.append([x, y, 0.0]) + return coords + diff --git a/others/readme.md b/others/readme.md new file mode 100644 index 0000000..512ced0 --- /dev/null +++ b/others/readme.md @@ -0,0 +1,10 @@ +pyTsai鏄竴涓娇鐢═sai鏂规硶缁撳悎blender鐨勬牎鍑嗘柟娉曘傚叾涓璼rc涓殑浠ｇ爜澶ч儴鍒嗗簲璇ユ槸鏉ヨ嚜Tsai鏈汉鎵鍐欙紝闈炵嚎鎬т紭鍖栭儴鍒嗕娇鐢ㄤ簡minpack杩涜鐨勩 + +鍙弬鑰冿細 +https://github.com/Csega/pyTsai + +https://icube-forge.unistra.fr/flarue/Larue.AcqPipe.public/-/tree/master/externals/tsai + +TsaiCpp鏄娇鐢╬yTsai杩涜涓浜涗紭鍖栵紝骞朵娇鐢–++璇█閲嶆柊鍐欑殑浠ｇ爜锛岄潪绾挎т紭鍖栭噰鐢ㄤ簡[Xtinc/matrix](https://github.com/Xtinc/matrix)鐨勫疄鐜般俆hreeStepOptimization鏂规硶灏辨槸pyTsai涓殑閭ｇ锛孎iveStepOptimization鏂规硶鏄弬鑰冧笓鍒╋細 + +鍖椾含鑸┖鑸ぉ澶у. 涓绉嶅熀浜庨忚鎴愬儚妯″瀷鏍囧畾鐨凜鍨嬭噦鍥惧儚鏍℃鏂规硶:CN200910087257.4[P]. 2009-11-18. diff --git a/output/output.pdf b/output/output.pdf index 9973371..bc2aea3 100644 Binary files a/output/output.pdf and b/output/output.pdf differ diff --git a/plot.py b/plot.py index f8ffca4..809c455 100644 --- a/plot.py +++ b/plot.py @@ -16,20 +16,20 @@ def plotPoints(points, image, title=''): fig, (ax) = plt.subplots(1) - fig.set_size_inches(6,6) + fig.set_size_inches(6, 6) - markersize=6 + markersize = 6 ax.imshow(image, alpha=0.4) x, y = zip(*map(lambda point: point.pixel, points)) - ax.plot(x, y, 'rx', markersize=markersize/math.sqrt(2), label='actual') - + ax.plot(x, y, 'rx', markersize=markersize / math.sqrt(2), label='actual') + px, py = zip(*map(lambda point: point.projectedPixel[:2], points)) - ax.plot(px, py, 'k+', markersize=markersize, label='projected') - + ax.plot(px, py, 'bx', markersize=markersize, label='projected') + ux, uy = zip(*map(lambda point: point.distortedPixel[:2], points)) - ax.plot(ux, uy, 'k+', markersize=markersize*2, label='distorted') - + ax.plot(ux, uy, 'k+', markersize=markersize * 2, label='distorted') + ax.set_title('%s' % title) ax.set_xlabel('x (px)') ax.set_ylabel('y (px)') @@ -41,7 +41,7 @@ def plotPoints(points, image, title=''): def plotStats(errorss, kappass, resolutions, labels, title=None): n = len(errorss) fig, (ax1, ax2) = plt.subplots(2, sharex=True) - fig.set_size_inches(6,6) + fig.set_size_inches(6, 6) if (title): ax1.set_title(title) @@ -50,9 +50,9 @@ def plotStats(errorss, kappass, resolutions, labels, title=None): ax2.set_ylabel('Mean error (pixels)') ax2.set_xlabel('Iteration (t)') - xaxis = range(-1,len(errorss[0])-1) + xaxis = range(-1, len(errorss[0]) - 1) - colors = cm.Dark2(np.linspace(0,1,n)) + colors = cm.Dark2(np.linspace(0, 1, n)) for i in range(n): kappas = kappass[i] @@ -63,9 +63,9 @@ def plotStats(errorss, kappass, resolutions, labels, title=None): ax1.plot(xaxis, kappas, marker='.', c=color, label=label) - #ax2.plot(map(lambda e: e.minmax[1], errors), ':', c=color) + # ax2.plot(map(lambda e: e.minmax[1], errors), ':', c=color) ax2.plot(xaxis, map(lambda e: e.mean, errors), marker='.', c=color, label=label) - #ax2.plot(map(lambda e: e.minmax[0], errors), ':', c=color) + # ax2.plot(map(lambda e: e.minmax[0], errors), ':', c=color) ax2.legend() @@ -77,28 +77,29 @@ def displayStereo(model): points = world['points'] fig = plt.figure() ax = fig.add_subplot(111) - fig.set_size_inches(6,6) + fig.set_size_inches(6, 6) - markersize=6 + markersize = 6 def displayCamera(c, label, color): params = c['params'] invRT = np.linalg.inv(params['RT']) - p = np.dot(invRT, [ 0.0, 0.0, 0.0, 1.0 ]) - px = np.dot(invRT, [ -10.0, 0.0, 0.0, 1.0 ]) - pz = np.dot(invRT, [ 0.0, 0.0, -10.0, 1.0 ]) - ax.plot([ p[0] ], [ p[2] ], 'k.', markersize=markersize, color=color, label=label) - dx = px-p - dz = pz-p - ax.arrow(p[0]-dx[0], p[2]-dx[2], dx[0]*2.0, dx[2]*2.0, head_width=markersize*0.3, head_length=markersize*0.4, fc=color, ec=color, label=label, linestyle=':') - ax.arrow(p[0]-dz[0], p[2]-dz[2], dz[0]*2.0, dz[2]*2.0, head_width=markersize*0.3, head_length=markersize*0.4, fc=color, ec=color, label=label, linestyle='-') + p = np.dot(invRT, [0.0, 0.0, 0.0, 1.0]) + px = np.dot(invRT, [-10.0, 0.0, 0.0, 1.0]) + pz = np.dot(invRT, [0.0, 0.0, -10.0, 1.0]) + ax.plot([p[0]], [p[2]], '.', markersize=markersize, color=color, label=label) + dx = px - p + dz = pz - p + ax.arrow(p[0] - dx[0], p[2] - dx[2], dx[0] * 2.0, dx[2] * 2.0, head_width=markersize * 0.3, + head_length=markersize * 0.4, fc=color, ec=color, label=label, linestyle=':') + ax.arrow(p[0] - dz[0], p[2] - dz[2], dz[0] * 2.0, dz[2] * 2.0, head_width=markersize * 0.3, + head_length=markersize * 0.4, fc=color, ec=color, label=label, linestyle='-') displayCamera(model['left'], 'Left Camera', 'g') displayCamera(model['right'], 'Right Camera', 'b') - - x = map(lambda point: point[0], points) - z = map(lambda point: point[2], points) + x = [p[0] for p in points] + z = [p[2] for p in points] ax.plot(x, z, 'r+', markersize=markersize, label='Calibration point') ax.set_xlabel('x (mm)') @@ -115,22 +116,22 @@ def displayStereoSide(model): points = world['points'] fig = plt.figure() ax = fig.add_subplot(111, projection='3d') - fig.set_size_inches(6,6) + fig.set_size_inches(6, 6) ax.view_init(30, 45) - markersize=6 + markersize = 6 def displayCamera(c, label, color): params = c['params'] invRT = np.linalg.inv(params['RT']) - p = np.dot(invRT, [ 0.0, 0.0, 0.0, 1.0 ]) - px = np.dot(invRT, [ 10.0, 0.0, 0.0, 1.0 ]) - py = np.dot(invRT, [ 0.0, 10.0, 0.0, 1.0 ]) - pz = np.dot(invRT, [ 0.0, 0.0, 10.0, 1.0 ]) - ax.plot([ p[0] ], [ p[1] ], 'k.', zs=[ p[2] ], markersize=markersize, color=color, label=label) - dx = px-p - dy = py-p - dz = pz-p + p = np.dot(invRT, [0.0, 0.0, 0.0, 1.0]) + px = np.dot(invRT, [10.0, 0.0, 0.0, 1.0]) + py = np.dot(invRT, [0.0, 10.0, 0.0, 1.0]) + pz = np.dot(invRT, [0.0, 0.0, 10.0, 1.0]) + ax.plot([p[0]], [p[1]], '.', zs=[p[2]], markersize=markersize, color=color, label=label) + dx = px - p + dy = py - p + dz = pz - p ax.quiver(p[0], p[1], p[2], dx[0], dx[1], dx[2], color=color) ax.quiver(p[0], p[1], p[2], dy[0], dy[1], dy[2], color=color) ax.quiver(p[0], p[1], p[2], dz[0], dz[1], dz[2], color=color) @@ -138,10 +139,10 @@ def displayCamera(c, label, color): displayCamera(model['left'], 'Left Camera', 'g') displayCamera(model['right'], 'Right Camera', 'b') + x = [p[0] for p in points] + y = [p[1] for p in points] + z = [p[2] for p in points] - x = map(lambda point: point[0], points) - y = map(lambda point: point[1], points) - z = map(lambda point: point[2], points) ax.plot(x, y, 'r+', zs=z, markersize=markersize, label='Calibration point') ax.set_xlabel('x (mm)') diff --git a/point.py b/point.py index 6361e01..836a816 100644 --- a/point.py +++ b/point.py @@ -1,17 +1,22 @@ #!/usr/bin/python ### # -# A Point holds a set of (homogeneous or cartesian) coordinates - each defines the position of the point in the appropriate basis. +# A Point holds a set of (homogeneous or cartesian) coordinates - +# each defines the position of the point in the appropriate basis. # Author: Samuel Bailey # ### from collections import namedtuple -#we're going to be using points a lot, so a namedtuple should give better performance than a hashmap + +# we're going to be using points a lot, so a namedtuple should give better performance than a hashmap def makePointClass(): - pointSchema = [ 'world', 'camera', 'pixel', 'projectedPixel', 'distortedPixel', 'sensor', 'projectedSensor', 'distortedSensor', 'normal', 'projectedNormal', 'distortedNormal' ] + pointSchema = ['world', 'camera', 'pixel', 'projectedPixel', 'distortedPixel', 'sensor', 'projectedSensor', + 'distortedSensor'] Point = namedtuple('Point', pointSchema) defaultPoint = Point(*map(lambda x: None, range(len(pointSchema)))) return Point, lambda dict: defaultPoint._replace(**dict) -Point, newPoint = makePointClass() \ No newline at end of file + + +Point, newPoint = makePointClass() diff --git a/transforms.py b/transforms.py index 9c76fd0..329db24 100644 --- a/transforms.py +++ b/transforms.py @@ -6,129 +6,110 @@ # ### -import math -import numpy as np from mmath import * +from scipy.optimize import minimize + ### Transform from world coordinates to pixel coordinates # 3d world coordinates (in mm) -> 3d camera coordinates (in mm) def worldToCamera(points, params, yOffset): - worldToCamera = np.dot(translationToHomogeneous([ params['tx'], params['ty']+yOffset, params['tz'] ]), rotationToHomogeneous(params['rotationMatrix'])) + worldToCamera = np.dot(translationToHomogeneous([params['tx'], params['ty'] + yOffset, params['tz']]), + rotationToHomogeneous(params['rotationMatrix'])) def transform(point): - return point._replace(camera = np.dot(worldToCamera, [ point.world[0], point.world[1], point.world[2], 1 ])) - return map(transform, points) + return point._replace(camera=np.dot(worldToCamera, [point.world[0], point.world[1], point.world[2], 1])) + return list(map(transform, points)) -# 3d camera coordinates (in mm) -> sensor coordinates (2d, in mm) -def projectPoints(points, f): - cameraToPlane = np.array([ [ f, 0.0, 0.0, 0.0 ], [ 0.0, f, 0.0, 0.0 ], [ 0.0, 0.0, 1.0, 0.0 ] ], np.float64) - def projectPoint(point): +# 3d camera coordinates (in mm) -> sensor coordinates (2d, in mm) +def projectPoints(points, pixelSize, resolution, f): + cameraToPlane = np.array([[f, 0.0, 0.0, 0.0], [0.0, f, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0]], np.float64) + def project_sensor(point): # perspective projection of the 3d point onto the plane at z=f p = np.dot(cameraToPlane, point.camera) - p = p / p[2] #perspective division + p = p / p[2] # perspective division # p is now a 2d vector from the center of the image sensor, in mm - return point._replace(projectedSensor = p) - - return map(projectPoint, points) + return point._replace(projectedSensor=p) + def project_pixel(point): + s = point.projectedSensor + p = np.array([s[0] / pixelSize[0] + resolution[0] / 2, s[1] / pixelSize[1] + resolution[1] / 2]) + return point._replace(projectedPixel=p) -# sensor coordinates (2d, in mm) -> normalised image coordinates -def sensorToNormal(points, pixelSize, resolution): - s2n = np.array([[2.0/(pixelSize[0]*resolution[0]),0.0,0.0],[0.0,2.0/(pixelSize[1]*resolution[1]),0.0],[0.0,0.0,1.0]], np.float64) - - def transform(point): - p = [ point.projectedSensor[0], point.projectedSensor[1], 1.0 ] - return point._replace(projectedNormal = np.dot(s2n, p)) - return map(transform, points) + points = list(map(project_sensor, points)) + return list(map(project_pixel, points)) # normalised image coordinates -> distorted normalised image coordinates -def distortPoints(points, kappa): - def dist(u, d=None, i=8): - if i == 0: - d = u - else: - d = dist(u, d, i-1) - - rd2 = (d[0]*d[0]) + (d[1]*d[1]) - correction = 1.0 / ( 1.0 - (kappa * rd2) ) - return np.multiply(u, [ correction, correction, 1.0 ]) - - def transform(point): - return point._replace(distortedNormal = dist(point.projectedNormal)) - return map(transform, points) - - -# (distorted) normalised image coordinates -> pixels -def normalToPixel(points, resolution): - n2p = np.array([[-resolution[0]/2.0,0.0,resolution[0]/2.0],[0.0,-resolution[1]/2.0,resolution[1]/2.0],[0.0,0.0,1.0]], np.float64) - - def transform(point): - p = [ point.projectedNormal[0], point.projectedNormal[1], 1.0 ] - d = [ point.distortedNormal[0], point.distortedNormal[1], 1.0 ] - return point._replace(projectedPixel = np.dot(n2p, p), distortedPixel = np.dot(n2p, d)) - return map(transform, points) - +def distortPoints(points, pixelSize, resolution, kappa): + def trans_distorted_sensor(point): + """ + x_u=x_d(1+k*r^2),y_u=y_d(1+k*r^2), where r^2 = (x_d)^2+(y_u)^2 + (x_u)^2+(y_u)^2 = [(x_d)^2+(y_u)^2]*[(1+k*r^2)^2] + then, u2=d2(1+k*d2)^2, now we need to know d2, that means we could minimize + [u2-d2(1+k*d2)^2]^2 + :param point: + :return: + """ + u = point.projectedSensor + ru2 = (u[0] * u[0]) + (u[1] * u[1]) + + obj = lambda x: (x * ((1 + kappa * x) ** 2) - ru2) ** 2 + res = minimize(obj, np.array([ru2]), method='Powell') + rd2 = res.x[0] + + correction = 1.0 / (1.0 + (kappa * rd2)) + return point._replace(distortedSensor=np.multiply(u, [correction, correction, 1.0])) + + def trans_distorted_pixel(point): + d = point.distortedSensor + return point._replace(distortedPixel=np.array([d[0] / pixelSize[0] + resolution[0] / 2, + d[1] / pixelSize[1] + resolution[1] / 2])) + + points = list(map(trans_distorted_sensor, points)) + return list(map(trans_distorted_pixel, points)) ### Transform from pixel to world coordinates - -# pixel coordinates -> normalised image coordinates -def pixelToNormal(points, resolution): - n2p = np.array([[-resolution[0]/2.0,0.0,resolution[0]/2.0],[0.0,-resolution[1]/2.0,resolution[1]/2.0],[0.0,0.0,1.0]], np.float64) - p2n = np.linalg.inv(n2p) - - def transform(point): - p = np.array([ point.pixel[0], point.pixel[1], 1.0 ], np.float64) #pixel coordinates to homogeneous - return point._replace(normal = np.dot(p2n, p)) # = [ xd, yd, 1.0 ] transform pixel coordinates to sensor coordinates (in mm, origin at center of camera) - return map(transform, points) - - # normalised image coordinates -> undistorted normalised image coordinates def undistortPoints(points, kappa): def transform(point): - x, y = point.normal[0], point.normal[1] - correction = 1.0 - ( kappa * ( (x*x) + (y*y) ) ) - return point._replace(normal = np.multiply(point.normal, [ correction, correction, 1.0 ])) - return map(transform, points) - - -# normalised image coordinates -> sensor coordinates (2d, in mm) -def normalToSensor(points, resolution, pixelSize): - s2n = np.array([[2.0/(pixelSize[0]*resolution[0]),0.0,0.0],[0.0,2.0/(pixelSize[1]*resolution[1]),0.0],[0.0,0.0,1.0]], np.float64) - n2s = np.linalg.inv(s2n) - - def transform(point): - p = np.array([ point.normal[0], point.normal[1], 1.0 ], np.float64) #pixel coordinates to homogeneous - return point._replace(sensor = np.dot(n2s, p)) # = [ xd, yd, 1.0 ] transform pixel coordinates to sensor coordinates (in mm, origin at center of camera) - return map(transform, points) + x, y = point.sensor[0], point.sensor[1] + correction = 1.0 + (kappa * ((x * x) + (y * y))) + return point._replace(sensor=np.multiply(point.sensor, [correction, correction, 1.0])) + return list(map(transform, points)) ### Compose a few of the above together to make things easier to read +def pixelToSensor(points, resolution, pixelSize, kappa=0.0): + def transform(point): + return point._replace(sensor=np.array([(point.pixel[0] - resolution[0] / 2) * pixelSize[0], + (point.pixel[1] - resolution[1] / 2) * pixelSize[1], 1])) + return undistortPoints(list(map(transform, points)), kappa) -def pixelToSensor(points, resolution, pixelSize, kappa=0.0): - return normalToSensor(undistortPoints(pixelToNormal(points, resolution), kappa), resolution, pixelSize) def sensorToPixel(points, pixelSize, resolution, kappa=0.0): - return normalToPixel(distortPoints(sensorToNormal(points, pixelSize, resolution), kappa), resolution) + return distortPoints(points, pixelSize, resolution, kappa) + def worldToSensor(points, params, pixelSize, resolution, yOffset, kappa=0.0): - return sensorToPixel(projectPoints(worldToCamera(points, params, yOffset), params['f']), pixelSize, resolution, kappa) + return sensorToPixel(projectPoints(worldToCamera(points, params, yOffset), pixelSize, resolution, params['f']), + pixelSize, resolution, kappa) -def worldToPixel(points, params, pixelSize, resolution, yOffset, kappa=0.0): - return sensorToPixel(projectPoints(worldToCamera(points, params, yOffset), params['f']), pixelSize, resolution, kappa) +def worldToPixel(points, params, pixelSize, resolution, yOffset, kappa=0.0): + return sensorToPixel(projectPoints(worldToCamera(points, params, yOffset), pixelSize, resolution, params['f']), + pixelSize, resolution, kappa) # Distance from the origin in camera coordinates to the origin in world coordinates (in mm) def cameraToWorldDistance(params, yOffset): - return np.linalg.norm([params['tx'], params['ty']-yOffset, params['tz']]) + return np.linalg.norm([params['tx'], params['ty'] - yOffset, params['tz']]) diff --git "a/\345\216\237\347\220\206.md" "b/\345\216\237\347\220\206.md" new file mode 100644 index 0000000..fe9d2e5 --- /dev/null +++ "b/\345\216\237\347\220\206.md" @@ -0,0 +1,1267 @@ +### Tsai + +Hall涓嶧augeras鏂规硶閮芥槸涓嶈冭檻浠讳綍鐣稿彉鐨勬儏鍐碉紝杩涜鐩告満鍐呭鍙傜殑姹傝В锛屾槸鐩存帴绾挎у垎瑙ｆ柟寮忋傝繕鏈変竴绉嶆槸鍙冭檻寰勫悜鐣稿彉鐨勬柟娉曚负Tsai锛岃娉ㄦ剰鐨勬槸杩欎釜鏂规硶**鍙冭檻寰勫悜鐣稿彉**锛屽苟涓斾富鐐(鍏夎酱鐨勪腑蹇)鏄粯璁ゅ湪鍥惧儚鐨勬涓績锛岃繖鏄疶sai鏂规硶鐨勪竴涓噸瑕佸墠鎻愩 + +鍦ㄤ箣鍓嶄笉鑰冭檻鐣稿彉鐨勬椂鍊欙紝绌洪棿鐐$(X_w,Y_w,Z_w)$鍦ㄧ浉鏈哄潗鏍囩郴涓嬬殑鎴愬儚鐐$P_d^C$涓庢棤鐣稿彉鐐$P_u^C$鏄竴涓偣锛屽洜涓轰笉鑰冭檻鐣稿彉銆傚湪娣诲姞涓婄暩鍙樺悗锛屾湁锛 + +$$ +X_u^C=X_d^C+\delta _x,Y_u^C=Y_d^C+\delta _y +$$ + +寰勫悜鐣稿彉鏄煎湪鎴愬儚涓績鍚戝鎴栬呭悜鍐呭彂鐢熺殑鐣稿彉锛屽鏋滀竴涓垚鍍忕偣鍦ㄥ浘鍍忓潗鏍囩郴涓嬶紝涓庡厜杞翠腑蹇冪殑璺濈涓$r=\sqrt{(X_d^{C})^2+(Y_d^{C})^2}$锛岄偅涔: + +$$ +\delta_x = X_d^C(k_1r^2+k_2r^4+k_3t^6+......),\delta_y = Y_d^C(k_1r^2+k_2r^4+k_3t^6+......) +$$ + +鐞嗚涓婏紝闃舵暟瓒婇珮锛岃〃杈惧緱瓒婂噯纭紝浣嗘槸涓鑸潵璁诧紝浣跨敤涓闃跺凡缁忓彲浠ヨ〃绀哄ぇ澶氭暟鐩告満鐨勫緞鍚戠暩鍙橈紝涔熷氨鏄$k_1\neq 0, k_i=0(i>1)$銆 + +閭ｄ箞 + +$$ +X_u^C=X_d^C(1+k_1r^2),Y_u^C=Y_d^C(1+k_1r^2) +$$ + +
+ +鍦ㄤ笂鍥句腑锛岀偣$P_w$鏄3D鐐癸紝閫氳繃灏勭嚎鎶曞奖鍦ㄦ垚鍍忛潰涓婄殑鐐逛负$P_u$锛岀暩鍙樺悗鍦ㄦ垚鍍忓钩闈笂鐨勭偣涓$P_d$锛岀敱浜庡彧鑰冭檻浜嗚繘琛岀暩鍙橈紝鎵浠$O_R$,$P_d$鍜$P_u$涓変釜鐐规槸鍦ㄥ悓涓鏉＄洿绾夸笂鐨勩傞鍏堟垜浠繕鏄冭檻娌℃湁鐣稿彉鐨勬儏鍐碉紝涔熷氨鏄$k_1=0$,$P_u$鍜$P_d$鏄竴涓偣銆傚湪鐩告満鍧愭爣绯讳笅瑙傚療锛屽彲浠ヨ涓$\vec{P_{oz}P_w}$涓$\vec{O_RP_u}$鏄钩琛岀殑锛屾墍浠$\vec{P_{oz}P_w}\times\vec{O_RP_u}=0$銆傞偅涔$x_u^cY_c-y_u^cX_c=0$銆 + +鍏朵腑锛$x_u^c,y_u^c$鍒嗗埆琛ㄧず鎴愬儚鐐瑰湪鐩告満鍧愭爣绯讳笅鐨勪綅缃潗鏍囷紝$X_c,Y_c$琛ㄧず涓夌淮鏍囪鐐瑰湪鐩告満鍧愭爣绯讳笅鐨勪綅缃潗鏍囥備富鐐瑰氨鍦ㄥ浘鍍忕殑涓績锛岄偅涔堟湁 + +$$ +\begin{cases} + +x_u^c=(x_i-u_0)*d_x\\ + +y_u^c=(y_i-v_0)*d_y\\ + +X_c=r_{11}X_w+r_{12}Y_w+r_{13}Z_w+T_x\\ + +Y_c=r_{21}X_w+r_{22}Y_w+r_{23}Z_w+T_y + +\end{cases}\tag{Tsai} +$$ + +鍋囪$\alpha=\frac{k_x}{k_y}=\frac{f/d_x}{f/d_y}=\frac{d_y}{d_x}$銆傞偅涔: + +$$ +(x_i-u_0)(r_{21}X_w+r_{22}Y_w+r_{23}Z_w+T_y) = (y_i-v_0)\alpha(r_{11}X_w+r_{12}Y_w+r_{13}Z_w+T_x) +$$ + +灏$x_i-u_0$璁颁负$x_i'$, $y_i-u_0$璁颁负$y_i'$銆傚皢涓婂紡鏁寸悊鎴愪负$\vec{A}\vec{X}=\vec{B}$鐨勫舰寮忥紝鍏朵腑 + +$$ +\begin{cases}\vec{A}=[x_i'X_w,x_i'Y_w,x_i'Z_w,x_i',-y_i'X_W,-y_i'Y_W,-y_i'Z_W,-y_i']^T \\ + +\vec{X}=[r_{21},r_{22},r_{23},T_y,\alpha r_{11},\alpha r_{12},\alpha r_{13},\alpha T_x]\\ + +\vec{B}=0 + +\end{cases} +$$ + +鐢变簬$\vec{B}=0$鏄綈娆℃柟绋嬶紝鎵浠ュ$A$杩涜SVD鍒嗚В锛屽緱鍒扮殑Vt鐨勬渶鍚庝竴琛屽悜閲忓嵆涓$X$鐨勮В銆傝繖閲岋紝搴旇涔熷彲浠ユ暣鐞嗘垚涓轰竴涓潪榻愭鐨勫舰寮忥紝鐒跺悗浣跨敤$X=(A^TA)^{-1}A^TB$鏉ユ眰瑙ｃ + +$$ +\begin{cases} + + \vec{A}=[x_i'X_w,x_i'Y_w,x_i'Z_w,-y_i'X_W,-y_i'Y_W,-y_i'Z_W,-y_i']^T\\ + + \vec{X}=[\frac{r_{21}}{T_y},\frac{r_{22}}{T_y},\frac{r_{23}}{T_y},\alpha \frac{r_{11}}{T_y},\alpha \frac{r_{12}}{T_y},\alpha \frac{r_{13}}{T_y},\alpha \frac{T_x}{T_y}]\\ + + \vec{B}=-x_i' + +\end{cases} +$$ + +閫氬父璁や负$u_0$鍜$v_0$灏辨槸鍍忕礌鍧愭爣绯荤殑涓績鐐瑰潗鏍囥傝繖閲岀殑鐭╅樀鏂圭▼鏈8涓湭鐭ユ暟锛屼竴涓偣瀵瑰彲浠ョ敓鎴愪竴涓柟绋嬶紝鎵浠ヨ嚦灏戦渶瑕佸叓涓偣鎵嶈兘姹傝В銆 + +褰撳緱鍒癤鍚庯紝闇瑕侀氳繃$v=[v_1,v_2,v_3]=[r_{21},r_{22},r_{23}]$鐨勬ā闀夸负1瀵$X$杩涜澶勭悊锛屽緱鍒扮殑$X$鎵嶆槸姝ｅ紡鐨勮В銆備絾鏄敱浜$[r_{21},r_{22},r_{23}]$鐨勬ā闀夸负1锛屽彧鑳藉綊涓鍖$X$鐨勫硷紝鑰屾病鏈夌鍙枫 + +鍋囧畾鏈変竴涓偣$P_w(X_w,Y_w,Z_w)$涓庢姇褰辩偣$P_d(x_i,y_i)$锛岄兘鍦ㄧ浉鏈哄潗鏍囩郴涓嬭瀵熴傜敱浜庣浉鏈哄潗鏍囩郴鐨勫師鐐瑰湪鎶曞奖闈腑蹇冪殑娉曞悜涓婏紝鎵浠ュ湪鐩告満鍧愭爣绯讳笅瑙傚療鐨勮瘽锛$X_c$涓$x_i$鐨勭鍙锋槸鐩稿悓鐨勶紝$Y_c$涓$y_i$鐨勭鍙蜂篃鏄浉鍚岀殑锛屽嵆鍙‘璁$X$瑙ｇ殑鏈缁堝舰寮忋 + +褰撴眰瑙ｅ埌杩欎竴姝ョ殑鏃跺欙紝杩樻湁$T_z$鍜$k_x,k_y$娌℃湁姹傝В鍑烘潵锛岀敱浜$\alpha=\frac{k_x}{k_y}$锛屾墍浠$k_x,k_y$鍙渶瑕佹眰鍑轰竴涓氨鍙互浜嗐 + +閫氳繃$(4)$鍙互寰楀埌锛 + +$$ +x_i-u_0 = k_x\frac{r_{11}X^w_i + r_{12}Y^w_i + r_{13}Z^w_i + T_x}{r_{31}X^w_i + r_{32}Y^w_i + r_{33}Z^w_i + T_z}\\ + +y_i-u_0 = k_y\frac{r_{21}X^w_i + r_{22}Y^w_i + r_{23}Z^w_i + T_y}{r_{31}X^w_i + r_{32}Y^w_i + r_{33}Z^w_i + T_z} +$$ + +鍚屾牱锛屽皢$x_i-u_0$璁颁负$x_i'$, $y_i-u_0$璁颁负$y_i'$銆傚崟鍙栫涓琛岋紝鏁寸悊鎴愪负锛 + +$$ +\begin{bmatrix} + +r_{11}X^w_i + r_{12}Y^w_i + r_{13}Z^w_i + T_x & -x_i' + +\end{bmatrix} + +\begin{bmatrix} + +k_y\\T_z + +\end{bmatrix}=x_i'(r_{31}X^w_i + r_{32}Y^w_i + r_{33}Z^w_i) +$$ + +鎴栬呭彇绗簩琛岋紝鏁寸悊鎴愪负锛 + +$$ +\begin{bmatrix} + +r_{21}X^w_i + r_{22}Y^w_i + r_{23}Z^w_i + T_y & -y_i' + +\end{bmatrix} + +\begin{bmatrix} + +k_y\\T_z + +\end{bmatrix}=y_i'(r_{31}X^w_i + r_{32}Y^w_i + r_{33}Z^w_i) +$$ + +灏嗘墍鏈夌殑鐐瑰潗鏍囧甫鍏ワ紝鏋勫缓瓒呭畾鏂圭▼杩涜鏈灏忎簩涔樻眰瑙ｏ紝寰楀埌$k_y,T_z$锛岄氳繃$\alpha$寰楀埌$k_y$銆 + +濡備笅涓鸿繘琛岀浉鏈烘牎鍑嗙殑C++浠ｇ爜锛屽叾涓殑 `m_ImageSize`涓哄浘鍍忕殑澶у皬銆 + +```cpp + +bool Tsai(std::vector& markers3DPoints, std::vector& image2DPoints) + +{ + + if (markers3DPoints.size() != image2DPoints.size()) + + { + + return false; + + } + + int size = markers3DPoints.size(); + + // compute r11,r12,r13,r21,r22,r23,Tx,Ty + + double offset_x = (m_ImageSize[0]-1.0) / 2.0; + + double offset_y = (m_ImageSize[1]-1.0) / 2.0; + + cv::Mat A = cv::Mat_(size, 8); + + for (size_t i = 0; i < size; i++) + + { + + A.at(i, 0) = (image2DPoints[i][0] - offset_x) * markers3DPoints[i][0]; + + A.at(i, 1) = (image2DPoints[i][0] - offset_x) * markers3DPoints[i][1]; + + A.at(i, 2) = (image2DPoints[i][0] - offset_x) * markers3DPoints[i][2]; + + A.at(i, 3) = (image2DPoints[i][0] - offset_x); + + A.at(i, 4) = -(image2DPoints[i][1] - offset_y) * markers3DPoints[i][0]; + + A.at(i, 5) = -(image2DPoints[i][1] - offset_y) * markers3DPoints[i][1]; + + A.at(i, 6) = -(image2DPoints[i][1] - offset_y) * markers3DPoints[i][2]; + + A.at(i, 7) = -(image2DPoints[i][1] - offset_y); + + } + + cv::Mat matD, matU, matVt; + + cv::SVD::compute(A, matD, matU, matVt); + + // normalize length and sign of V + + cv::Mat V = matVt.rowRange(7, 8); + + double gamma_abs = cv::norm(V.colRange(0, 3)); + + std::cout << "gamma_abs = " << gamma_abs << std::endl; + + V = V / gamma_abs; + + double sign = (image2DPoints[0][1] - offset_y) * ( + + V.at(0, 0) * markers3DPoints[0][0] + + + V.at(0, 1) * markers3DPoints[0][1] + + + V.at(0, 2) * markers3DPoints[0][2] + + + V.at(0, 3) + + ); + + if (sign<0) + + { + + V = -V; + + } + + // compute alpha, Tx,Ty, R + + double alpha = cv::norm(V.colRange(4, 7)); + + cv::Mat r2 = V.colRange(0, 3); + + cv::Mat r1 = V.colRange(4, 7) / alpha; + + cv::Mat r3 = r1.cross(r2); + + cv::Mat R; + + cv::vconcat(std::vector{r1, r2, r3}, R); + + double Ty = V.at(0, 3); + + double Tx = V.at(0, 7) / alpha; + + // compute fx,fy,Tz + + cv::Mat AA = cv::Mat_(size, 2); + + cv::Mat B = cv::Mat_(size, 1); + + for (size_t i = 0; i < size; i++) + + { + + cv::Mat matp3d = (cv::Mat_(1, 3) << markers3DPoints[i][0], + + markers3DPoints[i][1], markers3DPoints[i][2]); + + double xi = image2DPoints[i][0] - offset_x; + + AA.at(i, 0) = r1.dot(matp3d) + Tx; + + AA.at(i, 1) = -xi; + + + B.at(i, 0) = xi * r3.dot(matp3d); + + } + + cv::Mat matKxTz = (AA.t() * AA).inv() * AA.t() * B; + + double kx = matKxTz.at(0, 0); + + double Tz = matKxTz.at(1, 0); + + double ky = kx / alpha; + + /*********Show the result**********/ + + cv::Mat T = (cv::Mat_(3, 1) << Tx, Ty, Tz); + + std::cout << "kx = " << kx << std::endl << + + "ky = " << ky << std::endl << + + "R = " << R << std::endl << + + "T = " << T.t() << std::endl; + + std::cout << "camera pose = " << -R.t() * T << std::endl; + + return true; + +} + +``` + +鐒跺悗鑰冭檻鏈夌暩鍙樼殑鎯呭喌锛屼篃灏辨槸鐣稿彉绯绘暟$k_1\neq 0$,閭ｄ箞$(Tsai)$杩欎釜鏂圭▼缁勪笉鑳借繖涔堝啓浜嗭紝鐢变簬$(X_c,Y_c)$鏄┖闂村潗鏍囩偣$(X_w,Y_w,Z_w)$閫氳繃閫夋嫨骞崇Щ寰楀埌鐨勶紝鎵浠$(X_c,Y_c)$鏄病鏈変换浣曠暩鍙樼殑锛屼絾鏄浘鍍忎腑妫娴嬪緱鍒扮殑鍍忕礌鍧愭爣鐐$(x_i,y_i)$鏄寘鍚暩鍙樼殑锛$(x_i-u_0)*d_x$鍙兘鏄$x_d^c$锛岃屼笉鏄$x_u^c$锛屼絾鏄墠闈㈠凡缁忕煡閬撲簡$x_u^c = x_d^c(1+k_1r^2)$锛岀暩鍙樹笌闈炵暩鍙樼殑鍧愭爣瀛樺湪涓涓郴鏁板叧绯$1+k_1r^2$锛屾墍浠ュ湪鍥惧儚鐨勫潗鏍囦笂娣诲姞绯绘暟鍏崇郴鍗冲彲锛岃櫧鐒$r$涓殑鍊兼敹鍒颁簡$d_x,d_y$鐨勫奖鍝嶏紝浣嗘槸鍦ㄨ绠椾腑鍙互灏$r$绠鍖栦负$r^2=x_i'^2+y_i'^2$鏉ヨ绠椼 + +鎵浠$Tsai$鍙啓鎴愶細 + +$$ +\begin{cases} + +x_d^c=(x_i-u_0)*d_x\\ + +y_d^c=(y_i-v_0)*d_y\\ + +r^2 = (x_d^c)^2+(y_d^c)^2\\ + +x_u^c = x_d^c(1+k_1r^2)\\ + +y_u^c = y_d^c(1+k_1r^2)\\ + +X_c=r_{11}X_w+r_{12}Y_w+r_{13}Z_w+T_x\\ + +Y_c=r_{21}X_w+r_{22}Y_w+r_{23}Z_w+T_y + +\end{cases}\tag{Tsai with distortion} +$$ + +#### 绠鍖栬凯浠1 + +閫氳繃杩唬鐨勬柟寮忚绠$k_1$浠ュ強鍏朵粬鐨勭浉鏈哄弬鏁帮細 + +1. 棣栧厛浣跨敤闈犺繎涓績鐨勭偣閫氳繃蹇界暐鐣稿彉($k_1=0$)鐨勬儏鍐佃绠椾竴娆″弬鏁 +2. 鐒跺悗浣跨敤杩滅涓績鐨勭偣杩涜鎶曞奖璁＄畻鎶曞奖鐐 +3. 浣跨敤鎶曞奖鐐逛笌鍥惧儚涓婄殑鐐圭殑璺濈淇℃伅浼拌鍙傛暟$k_1$ +4. 灏嗗弬鏁$k_1$甯﹀叆锛岄噸鏂拌绠楁墍鏈夌偣鐨勬湁鐣稿彉鐨勭偣鍧愭爣 +5. 閲嶅涓婇潰姝ラ锛岀洿鍒$k_1$鏀舵暃鍒扮ǔ瀹氬 + +鍙互浣跨敤[busyyang/tsai-calibration](https://github.com/busyyang/tsai-calibration)鏂规硶涓殑鏂瑰紡杩涜涓婅堪鏁翠釜杩囩▼銆 + +鐢变簬 + +$$ +x_u^c = x_d^c(1+k_1r^2)\\ + +y_u^c = y_d^c(1+k_1r^2) +$$ + +鍏崇郴鐨勫瓨鍦紝褰撶涓娆″拷鐣ョ暩鍙樿绠楀嚭鏉ュ唴澶栧弬鍚庯紝鍙互閫氳繃3D鐐圭殑鎶曞奖寰楀埌娌℃湁鐣稿彉鐨勭偣$P_u^c(x_u^c,y_u^c)$锛屼互鍙婂瓨鍦ㄧ暩鍙樺湪鍥惧儚涓婃娴嬪緱鍒扮殑鐐$P_d^c(x_d^c,y_d^c)$锛岀偣鍧愭爣閮借〃绀烘槸鍦ㄧ浉鏈哄潗鏍囩郴涓嬨 + +瀵逛笂寮忚繘琛屽钩鏂圭浉鍔狅細 + +$$ +(x_u^c)^2+(y_u^c)^2 = [(x_d^c)^2+(y_d^c)^2][(1+k_1r^2)]^2 +$$ + +浠$u2=(x_u^c)^2+(y_u^c)^2$, $d2=(x_d^c)^2+(y_d^c)^2$, 鍙﹀鎴戜滑鐭ラ亾$r^2=(x_d^c)^2+(y_d^c)^2=d2$ + +鎵浠ユ湁锛 + +$$ +u2=d2(1+k\times d2)^2\\ + +\sqrt{\frac{u2}{d2}}=1+k\times d2\\ + +k=(1-\sqrt{\frac{u2}{d2}})/d2 +$$ + +鐢变簬$u2,d2$鍒嗗埆琛ㄧず鐣稿彉鍓嶅悗鐨勭偣鍒板浘鍍忎腑蹇冭窛绂荤殑骞虫柟锛屼袱涓暟瀛楃殑绗﹀彿鎬绘槸鐩稿悓鐨勶紝鎵浠ュ紑鏍瑰彿鐨勬椂鍊欎笉闇瑕佽冭檻璐熷彿鐨勯棶棰樸 + +```py + +def estimateKappaP(point): + + """ + + x_u = x_d(1+k*r^2) + + y_u = y_d(1+k*r^2) where r^2 = (x_d)^2 + (y_d)^2 + + + (x_u)^2 + (y_u)^2 = [(x_d)^2 + (y_d)^2] [(1+k*r^2)^2] + + note u2 = (x_u)^2 + (y_u)^2, and d2 = (x_d)^2 + (y_d)^2 + + so, u2 = d2[(1+k*d2)^2] + + then, k = (sqrt(u / d) - 1) / d2 + + """ + + u2 = (point.projectedSensor[0] * point.projectedSensor[0]) + ( + + point.projectedSensor[1] * point.projectedSensor[1]) + + d2 = (point.sensor[0] * point.sensor[0]) + (point.sensor[1] * point.sensor[1]) + + + d = math.sqrt(d2) + + u = math.sqrt(u2) + + k = (u / d - 1) / d2 + + return k + +``` + +鑰屽綋鎴戜滑宸茬煡鎶曞奖鐐癸紝鎯宠璁＄畻鐣稿彉鍚庣殑鐐瑰潗鏍囨椂鍊欙紝鎴戜滑宸茬煡浜$k$,$u2$,闇瑕佽绠楀嚭$d2$鎵嶈兘杩涜璁＄畻寰楀嚭鐣稿彉鍚庣殑鍧愭爣銆 + +$$ +u2=d2(1+k\times d2)^2\\ + +x_d^c =\frac{x_u^c}{(1+k_1\times d2)}\\ + +y_d^c =\frac{y_u^c}{(1+k_1\times d2)} +$$ + +鐒惰$d2$娌″姙娉曠洿鎺ヨ幏寰楋紝鍥犱负$P_d$鐨勫潗鏍囪繕娌℃湁銆備絾鏄瀵熷彲浠ュ彂鐜帮紝鍙互鎶$u2=d2(1+k\times d2)^2$鍐欐垚$y=x(1+k\times x)^2$鐨勫舰寮忥紝閫氳繃闈炵嚎鎬т紭鍖栫殑鏂规硶杩涜$d2$鐨勬眰瑙ｏ紝鏋勫缓$F=||d2(1+k\times d2)^2-u2||^2$锛屾眰浣垮緱$F$鏈灏忓寲鐨$d2$: $\argmin{F(d2)}$锛屽浜$d2$鐨勫垵鍊硷紝鍙互璁剧疆涓$u2$銆 + +```py + +import numpy as np + +from scipy.optimize import minimize + + +k = 0.072227403232112464 + +u2 = 0.97219326705259235 + + +obj = lambda x: (x * ((1 + k * x) ** 2) - u2) ** 2 + + +res = minimize(obj, np.array([u2]), method='Nelder-Mead') + +print(res.fun, '\n', res.success, '\n', res.x) + +``` + +浣跨敤杩欑鏂瑰紡锛岄鍏堜娇鐢ㄤ腑闂寸暩鍙樿緝灏忕殑鐐硅繘琛屽唴澶栧弬姹傚彇锛岀劧鍚庝及璁＄暩鍙樼郴鏁$k_1$锛屽悗缁皢鐣稿彉鑰冭檻杩涘幓閲嶅璁＄畻鍐呭鍙備笌$k_1$銆備竴寮濮嬬殑鏃跺欙紝璺濈涓績鐐硅繙鐨勬爣璁扮墿鐨勬姇褰辩偣涓庣暩鍙樺悗鐨勭偣鏈12pixel鐨勮宸紝缁忚繃澶氭杩唬鍚庢姇褰辩偣鍔犱笂鐣稿彉涓庢娴嬬殑寰楀埌鐨勭偣鐨勮宸负4.7pixel鐨勬牱瀛愩 + +
+ +```cpp + +bool CarmPositionAlgo::Tsai(std::vector& markers3DPoints, std::vector& image2DPoints,double k1) + +{ + + if (markers3DPoints.size() != image2DPoints.size()) + + { + + return false; + + } + + int size = markers3DPoints.size(); + + // compute r11,r12,r13,r21,r22,r23,Tx,Ty + + double offset_x = (m_ImageSize[0] - 1.0) / 2.0; + + double offset_y = (m_ImageSize[1] - 1.0) / 2.0; + + cv::Mat A = cv::Mat_(size, 8); + + for (size_t i = 0; i < size; i++) + + { + + double xi = (image2DPoints[i][0] - offset_x); + + double yi = (image2DPoints[i][1] - offset_y); + + double r2 = xi * m_ImageSpacing[0] * xi * m_ImageSpacing[0] + yi * yi * m_ImageSpacing[1] * m_ImageSpacing[1]; + + xi = xi * (1 + k1 * r2); + + yi = yi * (1 + k1 * r2); + + A.at(i, 0) = xi * markers3DPoints[i][0]; + + A.at(i, 1) = xi * markers3DPoints[i][1]; + + A.at(i, 2) = xi * markers3DPoints[i][2]; + + A.at(i, 3) = xi; + + A.at(i, 4) = -yi * markers3DPoints[i][0]; + + A.at(i, 5) = -yi * markers3DPoints[i][1]; + + A.at(i, 6) = -yi * markers3DPoints[i][2]; + + A.at(i, 7) = -yi; + + } + + cv::Mat matD, matU, matVt; + + cv::SVD::compute(A, matD, matU, matVt); + + // normalize length and sign of V + + cv::Mat V = matVt.rowRange(7, 8); + + double gamma_abs = cv::norm(V.colRange(0, 3)); + + V = V / gamma_abs; + + double sign = (image2DPoints[0][1] - offset_y) * ( + + V.at(0, 0) * markers3DPoints[0][0] + + + V.at(0, 1) * markers3DPoints[0][1] + + + V.at(0, 2) * markers3DPoints[0][2] + + + V.at(0, 3) + + ); + + if (sign<0) + + { + + V = -V; + + } + + // compute alpha, Tx,Ty, R + + double alpha = cv::norm(V.colRange(4, 7)); + + cv::Mat r2 = V.colRange(0, 3); + + cv::Mat r1 = V.colRange(4, 7) / alpha; + + cv::Mat r3 = r1.cross(r2); + + cv::Mat R; + + cv::vconcat(std::vector{r1, r2, r3}, R); + + double Ty = V.at(0, 3); + + double Tx = V.at(0, 7) / alpha; + + // compute fx,fy,Tz + + cv::Mat AA = cv::Mat_(size, 2); + + cv::Mat AAA = cv::Mat_(size, 2); + + cv::Mat B = cv::Mat_(size, 1); + + cv::Mat BB = cv::Mat_(size, 1); + + for (size_t i = 0; i < size; i++) + + { + + cv::Mat matp3d = (cv::Mat_(1, 3) << markers3DPoints[i][0], + + markers3DPoints[i][1], markers3DPoints[i][2]); + + double xi = image2DPoints[i][0] - offset_x; + + double yi = image2DPoints[i][1] - offset_y; + + double rs = xi * m_ImageSpacing[0] * xi * m_ImageSpacing[0] + yi * yi * m_ImageSpacing[1] * m_ImageSpacing[1]; + + xi = xi * (1 + k1 * rs); + + yi = yi * (1 + k1 * rs); + + AA.at(i, 0) = r1.dot(matp3d) + Tx; + + AA.at(i, 1) = -xi; + + B.at(i, 0) = xi * r3.dot(matp3d); + + + AAA.at(i, 0) = r2.dot(matp3d) + Ty; + + AAA.at(i, 1) = -yi; + + BB.at(i, 0) = yi * r3.dot(matp3d); + + + } + + //cv::Mat matKxTz = (AA.t() * AA).inv() * (AA.t() * B); + + //double kx = matKxTz.at(0, 0); + + //double Tz = matKxTz.at(1, 0); + + //double ky = kx / alpha; + + //double f = ky * m_ImageSpacing[1]; + + + cv::Mat matKyTz = (AAA.t() * AAA).inv() * (AAA.t() * BB); + + double ky = matKyTz.at(0, 0); + + double Tz = matKyTz.at(1, 0); + + double kx = ky * alpha; + + double f = kx * m_ImageSpacing[0]; + + + + /*********Show the result**********/ + + cv::Mat T = (cv::Mat_(3, 1) << Tx, Ty, Tz); + + std::cout << "kx = " << kx << std::endl << + + "ky = " << ky << std::endl << + + "R = " << R << std::endl << + + "T = " << T.t() << std::endl; + + cv::Mat camera_pose = -R.t() * T; + + std::cout << "camera pose = " << -R.t() * T << std::endl; + + + + // TODO: 杈撳嚭鍙傛暟杩樻病鏈夊畬鍏ㄨ緭鍑 + + m_CarmTransformParameters.f = f; + + m_CarmTransformParameters.kx = kx; + + m_CarmTransformParameters.ky = ky; + + m_CarmTransformParameters.u0 = offset_x; + + m_CarmTransformParameters.v0 = offset_y; + + m_CarmTransformParameters.rotation_matrix = R; + + m_CarmTransformParameters.translation_vector = T; + + m_CarmTransformParameters.camera_pose = camera_pose; + + m_CarmTransformParameters.image_spacing = cv::Vec3f(m_ImageSpacing[0], m_ImageSpacing[1], 1); + + + return true; + +} + + +bool CarmPositionAlgo::TsaiWithDistortion(std::vector& markers3DPointsInner, std::vector& image2DPointsInner, + + std::vector& markers3DPointsOutter, std::vector& image2DPointsOutter) + +{ + + const int iternum = 10; + + + + for (size_t i = 0; i < iternum; i++) + + { + + CarmTransformParameters param = GetCarmTransformParameters(); + + Tsai(markers3DPointsInner, image2DPointsInner, param.k1); + + param = GetCarmTransformParameters(); + + std::vector kappa; + + for (size_t j = 0; j < markers3DPointsOutter.size(); j++) + + { + + auto point = markers3DPointsOutter[j]; + + auto point_2d = image2DPointsOutter[j]; + + cv::Mat p3d = (cv::Mat_(3, 1) << point[0], point[1], point[2]); + + cv::Mat camera2plane = (cv::Mat_(3, 3) << param.f, 0, 0, 0, param.f, 0, 0, 0, 1); + + cv::Mat project_point = camera2plane * (param.rotation_matrix * p3d + param.translation_vector); + + project_point /= project_point.at(2, 0); + + + cv::Mat distort_point = (cv::Mat_(2, 1) << + + (point_2d[0] - param.u0) * param.image_spacing[0], + + (point_2d[1] - param.v0) * param.image_spacing[1]); + + double u2 = std::pow(cv::norm(project_point.rowRange(0, 2)), 2); + + double d2 = std::pow(cv::norm(distort_point), 2); + + double u = std::sqrt(u2); + + double d = std::sqrt(d2); + + double k = (u / d - 1) / d2; + + kappa.push_back(k); + + } + + double k1 = std::accumulate(kappa.begin(), kappa.end(), 0.0) / kappa.size(); + + std::cout << "Radial Distortion K1 = " << k1 << std::endl; + + m_CarmTransformParameters.k1 = k1; + + } + + return true; + +} + +``` + +#### 闈炵嚎鎬т紭鍖栨柟娉 + +鍦ㄨ繘琛屽畬涓娆′笉鑰冭檻鐣稿彉鐨凾sai鏂规硶鍚庯紝鐢变簬$f$,$T_z$鐨勫艰繕涓嶅鍑嗙‘锛$k_1$杩樻病鏈夊硷紝闇瑕佽繘涓姝ヤ紭鍖栥傝冭檻鐣稿彉鐨勬椂鍊欙紝鏈夛細 + +$$ +x_u^c=x_d^c(1+{\color{Red} k_1} r^2)=\frac{{\color{Red} f} (r_{11}X_w+r_{12}Y_w+r_{12}Z_w+Tx)}{r_{31}X_w+r_{32}Y_w+r_{32}Z_w+{\color{Red} Tz} } +$$ + +鍏朵腑锛岀孩鑹茬殑鍏冪礌鏄渶瑕侀噸鏂拌绠楃殑锛屾樉鐒讹紝鐢变簬$k_1$涓$T_z$鏈変箻绉叧绯伙紝涓嶆槸涓涓嚎鎬ч棶棰橈紝鑰冭檻褰撴垚涓涓潪绾挎ч棶棰樻眰瑙ｃ傛瀯閫狅細 + +$$ +F=||\frac{{\color{Red} f} (r_{11}X_w+r_{12}Y_w+r_{12}Z_w+Tx)}{r_{31}X_w+r_{32}Y_w+r_{32}Z_w+{\color{Red} Tz} }-x_d^c(1+{\color{Red} k_1} r^2)||^2 +$$ + +閭ｄ箞涔熷氨鏄眰浣垮緱$F$鐨勫兼渶灏忕殑$\{f,k_1,T_z\}$銆 + +浣跨敤https://github.com/simonwan1980/Tsai-Camera-Calibration/blob/master/Tsai/Tsai.m 涓殑鏂规硶锛屽湪姹傚嚭鍐呭鍙傚悗锛屼娇鐢╢minsearch鏂规硶姹$f,Tz,k_1$鐨勫噯纭硷紝杩欐牱鍙渶瑕佹眰涓娆″唴澶栧弬銆俧minsearch鐨勫疄鐜板彲浠ュ弬鑰僪ttps://github.com/Xtinc/matrix 杩欎釜repo涓殑瀹炵幇锛岃繖涓猺epo涓殑瀹炵幇鍙互杩涜绫讳技$y=2x_1x_2+x_1^2+x_1(x_2^2+5)^2$鐨勬渶灏忓寲浼樺寲闂锛岀敱浜庢垜浠殑鍐欏嚭鏉ョ殑浼樺寲鏂圭▼甯﹁娴嬪弬鏁帮紝濡$d=cx_1x_2+x_1^2+bx_1(x_2^2+a)^2$,涓嶈兘鐩存帴浣跨敤Xtinc鐨勬柟娉曪紝鍙互灏嗘墍鏈夎娴嬪艰繘琛屽钩鏂规眰鍜岀劧鍚庝綔涓轰紭鍖栨柟绋嬨 + +**Xtinc/matrix瀹炵幇锛** + +闈炵嚎鎬т紭鍖栨柟娉曠殑鏁板瀹炵幇姣旇緝澶嶆潅锛岀洿鎺ヤ娇鐢ㄤ簡[Xtinc/matrix](https://github.com/Xtinc/matrix)瀹炵幇鐨勫簱锛岀敱浜庤繖涓彧闇瑕佸ご鏂囦欢锛岄潪甯歌交閲忕骇銆 + +```cpp + +#include "matrixs/optimization.hpp"// for non-linear optimization + +#include "matrixs/matrixs.hpp"// for non-linear optimization + + ...... + + Tsai(markers3DPointsInner, image2DPointsInner, 0); // 鍒濆鍖栫暩鍙樼郴鏁発1=0璁＄畻鍙傛暟 + + CarmTransformParameters param = GetCarmTransformParameters(); + + markers3DPointsOutter.insert(markers3DPointsOutter.begin(), markers3DPointsInner.begin(), markers3DPointsInner.end()); + + image2DPointsOutter.insert(image2DPointsOutter.begin(), image2DPointsInner.begin(), image2DPointsInner.end()); + + std::vector xc, yc; // 鎵鏈夌偣鐨剎,y鍧愭爣(鍦ㄧ浉鏈哄潗鏍囩郴涓) + + std::transform(image2DPointsOutter.begin(), image2DPointsOutter.end(), std::back_inserter(xc), + + [=](auto p) {return (p[0] - param.u0) * param.image_spacing[0]; }); + + std::transform(image2DPointsOutter.begin(), image2DPointsOutter.end(), std::back_inserter(yc), + + [=](auto p) {return (p[1] - param.v0) * param.image_spacing[1]; }); + + double tx = param.translation_vector.at(0, 0); + + double ty = param.translation_vector.at(1, 0); + + + auto r1 = param.rotation_matrix.rowRange(0, 1); + + auto r2 = param.rotation_matrix.rowRange(1, 2); + + auto r3 = param.rotation_matrix.rowRange(2, 3); + + std::vector r1p, r2p, r3p;// 棰勫厛璁＄畻r1,r2,r3涓(X_w,Y_w,Z_w)鐨勪箻绉紝鏂逛究鍚庣画璁＄畻 + + std::transform(markers3DPointsOutter.begin(), markers3DPointsOutter.end(), std::back_inserter(r1p), + + [=](auto p) {return r1.at(0, 0) * p[0] + r1.at(0, 1) * p[1] + r1.at(0, 2) * p[2]; }); + + std::transform(markers3DPointsOutter.begin(), markers3DPointsOutter.end(), std::back_inserter(r2p), + + [=](auto p) {return r2.at(0, 0) * p[0] + r2.at(0, 1) * p[1] + r2.at(0, 2) * p[2]; }); + + std::transform(markers3DPointsOutter.begin(), markers3DPointsOutter.end(), std::back_inserter(r3p), + + [=](auto p) {return r3.at(0, 0) * p[0] + r3.at(0, 1) * p[1] + r3.at(0, 2) * p[2]; }); + + + // 浼樺寲鐩爣鏂圭▼锛屽鎵鏈夌殑瑙傛祴鐐癸紝璁＄畻F鐨勫嚱鏁板硷紝姹傚拰鍚庤繑鍥炪 + + auto obj = [=](const ppx::MatrixS<3, 1>& x) + + { + + double k1 = x[0]; + + double f = x[1]; + + double tz = x[2]; + + double value = 0, vx = 0, vy = 0; + + for (size_t i = 0; i < markers3DPointsOutter.size(); i++) + + { + + double r = xc[i] * xc[i] + yc[i] * yc[i]; + + vx += std::pow(xc[i] * (1 + k1 * r) * (r3p[i] + tz) - f * (r1p[i] + tx), 2); + + vy += std::pow(yc[i] * (1 + k1 * r) * (r3p[i] + tz) - f * (r2p[i] + ty), 2); + + } + + value = vx + vy; + + + std::cout << "Value = " << value; + + std::cout << "\tk1 = " << k1; + + std::cout << "\tf = " << f; + + std::cout << "\ttz = " << tz << std::endl; + + + return value; + + }; + + ppx::MatrixS<3, 1> x0{ 0.0,param.f,param.translation_vector.at(2, 0) };// 浣跨敤Tsai鐨勮绠楃粨鏋滀綔涓哄垵濮嬪弬鏁 + + auto res = ppx::fminunc(obj, x0);// 杩涜闈炵嚎鎬т紭鍖 + + std::cout << res.x << std::endl; + + ...... + +``` + +**ITK Powell瀹炵幇锛** + +杩欎釜瀹炵幇鐨勮繃绋嬶紝涔熷彲浠ョ洿鎺ヤ娇鐢↖TK搴擄紝瀹炵幇鍙互鍙傝僛PowellOptimizerTest](https://github.com/InsightSoftwareConsortium/ITK/blob/master/Modules/Numerics/Optimizers/test/itkPowellOptimizerTest.cxx). 棣栧厛闇瑕侀氳繃 `itk::SingleValuedCostFunction`缁ф壙涓涓被鍑烘潵鍐欑洰鏍囧嚱鏁帮紝绫讳腑鐨 `GetValue`鍑芥暟涓氨鍐欏叿浣撶殑瀹炵幇锛岀敱浜庡湪璁＄畻涓繕闇瑕佷娇鐢ㄥ埌鍏朵粬鐨勪俊鎭紝閫氳繃Set鍑芥暟杩涜璁剧疆銆傜敱浜庢墦绠椾娇鐢≒owell浼樺寲鍣紝鎵浠ヤ笉闇瑕佸疄鐜 `GetDerivative`閲岄潰鐨勫唴瀹癸紝濡傛灉鏄娇鐢ㄦ搴︾浉鍏崇殑浼樺寲鍣紝杩樻槸闇瑕佸疄鐜拌繖涓噷闈㈢殑鍐呭鐨勩 + +```cpp + +#include + +#include + +#include + + +class NonLinearFK1Tz :public itk::SingleValuedCostFunction + +{ + +public: + + using Self = NonLinearFK1Tz; + + using Superclass = itk::SingleValuedCostFunction; + + using Pointer = itk::SmartPointer; + + using ConstPointer = itk::SmartPointer; + + itkNewMacro(Self); + + itkTypeMacro(NonLinearFK1Tz, SingleValuedCostFunction); + + + enum + + { + + SpaceDimension = 3 + + }; + + + using ParametersType = Superclass::ParametersType; + + using DerivativeType = Superclass::DerivativeType; + + using MeasureType = Superclass::MeasureType; + + + NonLinearFK1Tz() = default; + + + + void + + GetDerivative(const ParametersType&, DerivativeType&) const override + + {} + + + MeasureType + + GetValue(const ParametersType& parameters) const override + + { + + double k1 = parameters[0]; + + double f = parameters[1]; + + double tz = parameters[2]; + + + MeasureType measure = 0; + + MeasureType vx = 0, vy = 0; + + for (size_t i = 0; i < m_Points3D.size(); i++) + + { + + double r = m_Xc[i] * m_Xc[i] + m_Yc[i] * m_Yc[i]; + + double r1p = m_r1[0] * m_Points3D[i][0] + m_r1[1] * m_Points3D[i][1] + m_r1[2] * m_Points3D[i][2]; + + double r2p = m_r2[0] * m_Points3D[i][0] + m_r2[1] * m_Points3D[i][1] + m_r2[2] * m_Points3D[i][2]; + + double r3p = m_r3[0] * m_Points3D[i][0] + m_r3[1] * m_Points3D[i][1] + m_r3[2] * m_Points3D[i][2]; + + vx += std::pow(m_Xc[i] * (1 + k1 * r) * (r3p + tz) - f * (r1p + m_Tx), 2); + + vy += std::pow(m_Yc[i] * (1 + k1 * r) * (r3p + tz) - f * (r2p + m_Ty), 2); + + } + + measure = vx + vy; + + + std::cout << "Value = " << measure; + + std::cout << "\tk1 = " << k1; + + std::cout << "\tf = " << f; + + std::cout << "\ttz = " << tz << std::endl; + + + return measure; + + } + + + unsigned int + + GetNumberOfParameters() const override + + { + + return SpaceDimension; + + } + + + void SetPoints3D(std::vector arg) { m_Points3D = arg; } + + void SetR1(std::vector arg) { m_r1 = arg; } + + void SetR2(std::vector arg) { m_r2 = arg; } + + void SetR3(std::vector arg) { m_r3 = arg; } + + void SetXc(std::vector arg) { m_Xc = arg; } + + void SetYc(std::vector arg) { m_Yc = arg; } + + void SetTx(double arg) { m_Tx = arg; } + + void SetTy(double arg) { m_Ty = arg; } + + +private: + + std::vector m_Points3D; + + std::vector m_Points2D; + + std::vector m_r1; + + std::vector m_r2; + + std::vector m_r3; + + std::vector m_Xc; + + std::vector m_Yc; + + double m_Tx; + + double m_Ty; + + +}; + +``` + +鐒跺悗灏 `NonLinearFK1Tz`浣滀负CostFunction璁剧疆缁 `itk::PowellOptimizer`浼樺寲鍣ㄥ嵆鍙 + +```cpp + +#include "OptimizeFK1Tz.h" + +#include + + ...... + + Tsai(markers3DPointsInner, image2DPointsInner, 0); + + CarmTransformParameters param = GetCarmTransformParameters(); + + + markers3DPointsOutter.insert(markers3DPointsOutter.begin(), markers3DPointsInner.begin(), markers3DPointsInner.end()); + + image2DPointsOutter.insert(image2DPointsOutter.begin(), image2DPointsInner.begin(), image2DPointsInner.end()); + + + using OptimizerType = itk::PowellOptimizer; + + auto itkOptimizer = OptimizerType::New(); + + auto costFunction = NonLinearFK1Tz::New(); + + + + std::vector xc, yc; + + std::transform(image2DPointsOutter.begin(), image2DPointsOutter.end(), std::back_inserter(xc), + + [=](auto p) {return (p[0] - param.u0) * param.image_spacing[0]; }); + + std::transform(image2DPointsOutter.begin(), image2DPointsOutter.end(), std::back_inserter(yc), + + [=](auto p) {return (p[1] - param.v0) * param.image_spacing[1]; }); + + auto r1 = param.rotation_matrix.rowRange(0, 1); + + auto r2 = param.rotation_matrix.rowRange(1, 2); + + auto r3 = param.rotation_matrix.rowRange(2, 3); + + + + costFunction->SetPoints3D(markers3DPointsOutter); + + costFunction->SetTx(param.translation_vector.at(0, 0)); + + costFunction->SetTy(param.translation_vector.at(1, 0)); + + costFunction->SetXc(xc); + + costFunction->SetYc(yc); + + costFunction->SetR1({ r1.at(0,0),r1.at(0,1) ,r1.at(0,2) }); + + costFunction->SetR2({ r2.at(0,0),r2.at(0,1) ,r2.at(0,2) }); + + costFunction->SetR3({ r3.at(0,0),r3.at(0,1) ,r3.at(0,2) }); + + + + itkOptimizer->SetCostFunction(costFunction); + + + using ParameterType = NonLinearFK1Tz::ParametersType; + + const unsigned int spaceDimension = costFunction->GetNumberOfParameters(); + + ParameterType initialPosition(spaceDimension); + + initialPosition[0] = 0; + + initialPosition[1] = param.f; + + initialPosition[2] = param.translation_vector.at(2, 0); + + + + OptimizerType::ScalesType scales(spaceDimension); + + scales[0] = 1000; + + scales[1] = 1; + + scales[2] = 1; + + itkOptimizer->SetScales(scales); + + + + itkOptimizer->SetMaximize(false); + + itkOptimizer->SetMaximumIteration(100); + + itkOptimizer->SetMaximumLineIteration(5); + + + itkOptimizer->SetInitialPosition(initialPosition); + + try + + { + + itkOptimizer->StartOptimization(); + + } + + catch (const std::exception&) + + { + + std::cout << "Exception thrown ! " << std::endl; + + } + + ParameterType finalPosition = itkOptimizer->GetCurrentPosition(); + + std::cout << "Solution = ("; + + std::cout << finalPosition[0] << ','; + + std::cout << finalPosition[1] << ','; + + std::cout << finalPosition[2] << ')' << std::endl; + + ...... + +``` + +### Tsai with LM + +杩欓噷鐨$k_1$涓庡墠闈sai璁烘枃涓笉澶竴鏍凤紝鍦═sai鐨勬枃绔犱腑锛 + +$$ +X_u=X_d+\delta_x\\ + +Y_u=Y_d+\delta_y\\ + +\delta_x=X_d(k_1r^2+k_2r^4+...)\\ + +\delta_y=Y_d(k_1r^2+k_2r^4+...) +$$ + +鍏朵腑$(X_d,Y_d)$鏄暩鍙樺浘鍍忓潗鏍囥$(X_u,Y_u)$鏄病鏈夌暩鍙樼殑鍥惧儚鍧愭爣锛$r=\sqrt{X_d^2+Y_d^2}$. + +鑰屼笅闈㈢殑鎻忚堪鍒欐槸涓嶽OpenCV](https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html)鐩稿悓锛 + +$$ +X_d=X_u+\delta_x\\ + +Y_d=Y_u+\delta_y\\ + +\delta_x=X_u(k_1r^2+k_2r^4+...)\\ + +\delta_y=Y_u(k_1r^2+k_2r^4+...) +$$ + +鍏朵腑$r=\sqrt{X_u^2+Y_u^2}$. + +鎬濊矾锛 + +- **Step1**: 涓嶈冭檻鍥惧儚浠讳綍鐣稿彉鐨勬儏鍐典笅锛岄鍏堜娇鐢═sai鏂规硶瀵瑰弬鏁拌繘琛屾眰瑙ｃ +- **Step2**: 浼樺寲$d_x,d_y,T_z,u_0,v_0,k_1,k_2$, 鐢变簬鎴戜滑鑰冭檻$f$鏄浐瀹氱殑锛屽唴鍙傜郴鏁颁腑$k_x=f/d_x,k_y=f/d_y$鍙兘寰楀埌$k_x,k_y$, 缁忔祴璇曪紝鎴戜滑鍥哄畾$f$姹$d_x,d_y$鏇村姞绗﹀悎Carm鐨勭粨鏋滐紝鍦ㄥ叾浠栨枃鐚腑澶氳涓$d_x,d_y$涓竴涓槸鍥哄畾鐨勶紝浼樺寲$f$鍜$d_x/d_y$鐨勬瘮渚嬪笺 + + 鏁版嵁鐐瑰湪鐩告満鍧愭爣绯讳笅鐨勮〃绀哄彲浠ュ啓涓猴細 + + $\begin{bmatrix} + + X_c\\Y_c\\Z_c + + \end{bmatrix} = R + + \begin{bmatrix} + + X_w\\Y_w\\Z_w + + \end{bmatrix} + + + \begin{bmatrix} + + T_x\\T_y\\T_z + + \end{bmatrix}$ + + 鎶婃暟鎹偣鎶曞奖鍒板浘鍍忎笂锛岀敱浜庡浘鍍忚窛绂讳负$f$: + + $\begin{cases} + + X_u= \frac{f}{Z_c}X_c=f\frac{R_1X_w+T_x}{R_3Z_w+T_z}\\ + + Y_u= \frac{f}{Z_c}Y_c=f\frac{R_2Y_w+T_y}{R_3Z_w+T_z} + + \end{cases}$ + + 鍦ㄤ粎鑰冭檻寰勫悜鐣稿彉鐨勬儏鍐典笅锛屾湁锛 + + $\begin{cases} + + X_d=X_u+\delta_x=X_u(1+k_1r^2+k_2r^4+...)\\ + + Y_d=Y_u+\delta_y=Y_u(1+k_1r^2+k_2r^4+...)\\ + + \text{where: } r^2=X_u^2+Y_u^2\end{cases}$ + + 姝ゆ椂锛$(X_d,Y_d)$鏄暩鍙樺浘鍍忓潗鏍囥$(X_u,Y_u)$鏄病鏈夌暩鍙樼殑鍥惧儚鍧愭爣锛屽潎鍦ㄧ墿鐞嗙┖闂翠笅锛屽崟浣嶅潎鏄 `mm`. 灏嗗瓨鍦ㄧ暩鍙樼殑$(X_i,Y_i)$杞崲鍒板浘鍍忓潗鏍囨椂锛 + + $ + + \begin{cases} + + X_d = (X_i-u_0)d_x=X_u+\delta_x=X_u(1+k_1r^2+k_2r^4+...)\\ + + Y_d = (Y_i-v_0)d_y=Y_u+\delta_y=Y_u(1+k_1r^2+k_2r^4+...) + + \end{cases}$ + + 甯﹀叆鎵鏈変腑闂村彉閲忥細 + + $ + + \begin{cases} + + (X_i-{\color{red}{u_0}}){\color{red}{d_x}} = f\frac{R_1X_w+T_x}{R_3Z_w+{\color{red}{T_z}}}(1+{\color{red}{k_1}}r^2+{\color{red}{k_2}}r^4+...)\\ + + (Y_i-{\color{red}{v_0}}){\color{red}{d_y}} = f\frac{R_2X_w+T_y}{R_3Z_w+{\color{red}{T_z}}}(1+{\color{red}{k_1}}r^2+{\color{red}{k_2}}r^4+...)\\ + + \text{where: }r^2=X_u^2+Y_u^2=(f\frac{R_1X_w+T_x}{R_3Z_w+{\color{red}{T_z}}})^2+(f\frac{R_2Y_w+T_y}{R_3Z_w+{\color{red}{T_z}}})^2 + + \end{cases}$ + + 绾㈣壊鐨勭鍙疯〃绀烘槸闇瑕佷紭鍖栫殑鍐呭锛屾樉鐒惰繖鏄竴涓潪绾挎х殑闂锛屽鏋滄妸涓婂紡鍐欐垚$A=B$鍜$C=D$鐨勫舰寮忥紝灏卞彲浠ュ啓鍑轰紭鍖栫殑鐩爣鍑芥暟涓猴細 + + $J=||A-B||^2+||C-D||^2$ + + 鍏朵腑锛$d_x,d_y,T_z$鐨勫垵濮嬪间娇鐢═sai鏂规硶鑾峰緱鐨勭粨鏋滐紝$u_0,v_0$鐨勫垵濮嬪间娇鐢ㄥ浘鍍忓ぇ灏忕殑涓鍗婂嵆鍙紝$k_1,k_2$鐨勫垵濮嬪间负0銆 +- **Step3**: 浼樺寲澶栧弬涓庣暩鍙樼郴鏁帮細 + + 澶栧弬涓昏鏄$R,T$褰㈡垚鐨勶紝鐣稿彉绯绘暟鎸夌収OpenCV鐨勫啓娉曟湁锛$R,T,k_1{\color{red}(,k_2)},p_1,p_2,s_1{\color{red}(,s_2)},s_3{\color{red}(,s_4)}$锛屽叾涓孩鑹叉嫭鍙峰唴鐨勫彲蹇界暐缃浂銆 + + $\begin{cases} + + x_u=\frac{R_{11}X_w+R_{12}Y_w+R_{13}Z_w+T_x}{R_{31}X_w+R_{32}Y_w+R_{33}Z_w+T_z},y_u=\frac{R_{21}X_w+R_{22}Y_w+R_{23}Z_w+T_y}{R_{31}X_w+R_{32}Y_w+R_{33}Z_w+T_z}\\ + + x^{''}=x_u(1+k_1r^2+k_2r^4)+2p_1x_uy_u+p_2(r^2+2x_u^2)+s_1r^2+s_2r^4\\ + + y^{''}=y_u(1+k_1r^2+k_2r^4)+p_1(r^2+2y_u^2)+2p_2x_uy_u+s_3r^2+s_4r^4\\ + + u \rightarrow \frac{f}{d_x}x^{''}+c_x,v \rightarrow \frac{f}{d_y}y^{''}+c_y\\ + + ......\\ + + J=\sqrt{||u-(\frac{f}{d_x}x^{''}+c_x)||^2+||v-(\frac{f}{d_y}y^{''}+c_y)||^2}\\ + + \text{where: } r^2=x_u^2+y_u^2 + + \end{cases}$ + + 瀵逛簬$R$鐭╅樀锛屽彲浠ュ皢$R$鐭╅樀鍒嗚В涓轰笁涓搴︼紝瀵硅搴﹁繘琛屼紭鍖栵紝鐒跺悗閫氳繃瑙掑害涓庣煩闃电殑鐩镐簰鍏崇郴杩涜杞崲銆 +- **Step4:** 浼樺寲鍐呭弬 + + 鍐呭弬涓昏鏄$k_x,k_y,u_0,v_0$鍥涗釜鍙傛暟锛岃冭檻鍒$k_x = f/d_x, k_y=f/d_y$,瀵$d_x,d_y,u_0,v_0$鍥涗釜鍙傛暟杩涜杩唬浼樺寲锛屼娇鐢ㄧ殑鍏紡杩樻槸濡係tep3涓竴鏍枫 +- **Step5:** 寰幆杩唬 + + 閲嶅姝ラ3-4鐩村埌鏀舵暃鍒扮洰鏍囥