本帖最后由 chlh_jd 于 2014-3-12 22:03 编辑
感谢高飞大师的总结!
Lee Mac 大师写了个5点拟合椭圆的程序:http://www.lee-mac.com/5pointellipse.html
我写了个最小二乘法拟合椭圆源码,欢迎大家批评指正:
;;最小二乘法拟合椭圆
- ;;;-------------------------------------------------------------
- (defun LS-Pts->Ellipse (lst / n arg_mat res_vec a b
- c d e f res xc yc aa bb
- ang co si co2 si2)
- ;; by GSLS(SS)
- ;; Pointset return Ellipse by Least Square method
- ;; 2014-01-02
- (if (> (setq n (length lst)) 4)
- (progn
- (mapcar (function (lambda (p / x y)
- (setq x (car p)
- y (cadr p))
- (setq arg_mat (cons (list (* x y) (* y y) x y 1.)
- arg_mat)
- res_vec (cons (* -1. x x) res_vec))))
- lst)
- (setq res (solve-odeq arg_mat res_vec))
- (mapcar (function set) (quote (a b c d e f)) (cons 1. res))
- (if (= a c)
- (setq ang _pi2)
- (setq ang (* 0.5 (atan (/ b (- a c))))))
- (if (and (/= f 0) (/= (setq delt (- (* 4. c) (* b b))) 0))
- (progn
- (setq xc (/ (- (* b e) (* 2. c d)) delt)
- yc (/ (- (* b d) (* 2. e)) delt)
- co (cos ang)
- si (sin ang)
- co2 (* co co)
- si2 (* si si)
- bb (sqrt
- (abs (/ (- (* (- (* f co2)
- (* (+ (* xc xc)
- (* b xc yc)
- (* c yc yc))
- co2))
- (- (* 2 xc si2)
- (* 2 yc co si)
- (* (+ xc xc (* b yc)) si2)))
- (* (- (* f si2)
- (* (+ (* xc xc)
- (* b xc yc)
- (* c yc yc))
- si2))
- (- (* 2 xc co2)
- (* -2 yc co si)
- (* (+ xc xc (* b yc)) co2))))
- (+ (* 2 xc co2)
- (* 2 yc co si)
- (- (* (+ xc xc (* b yc)) co2))))))
- aa (*
- (sqrt
- (abs (- (/ (+ (* 2 xc co2)
- (* 2 yc co si)
- (* -1 (+ xc xc (* b yc)) co2))
- (- (* 2 xc si2)
- (* 2 yc co si)
- (* (+ xc xc (* b yc)) si2))))))
- bb)
- )
- (list (list xc yc) (polar (list 0 0) ang aa) (/ bb aa)))))
- ))
通用最小二乘法函数
- (defun solve-odeq (arg_mat res_vector / mat)
- ;; Universal least squares method
- ;; arg_mat -- Parameters Matrix of Overdetermined Equations
- ;; res_vector -- Right term of Overdetermined Equations
- ;; by GSLS(SS) 2013.11.22
- (setq mat ([*] ([trp] arg_mat) arg_mat)
- mat ([inv] mat))
- (mxv ([*] mat ([trp] arg_mat)) res_vector))
- ;; Use function ---------------------------------------------
- ;; DotProduct
- (defun vxv (v1 v2) (apply (function +) (mapcar (function *) v1 v2)))
- ;; Matrix Vector Multiply
- (defun mxv (m v) (mapcar (function (lambda (r) (vxv r v))) m))
- ;; Transpose matrix
- (defun [trp] (a) (apply (function mapcar) (cons (function list) a)))
- ;; Matrix is multiplied by a matrix
- (defun [*] (m q) (mapcar (function (lambda (r) (mxv ([trp] q) r))) m))
- ;; To get Unit diagonal matrix
- (defun [I] (d / i r n m)
- (setq i d)
- (while (<= 0 (setq i (1- i)))
- (setq n d
- r nil)
- (while (<= 0 (setq n (1- n)))
- (setq r (cons (if (= i n)
- 1.0
- 0.0)
- r)))
- (setq m (cons r m))))
- ;;; gile-inverse-matrix (gile) 2009/03/17
- ;; uses the gauss-jordan elimination method to calculate the inverse matrix of any dimension square matrix
- ;; argument : a square matrix
- ;; return : the inverse matrix (or nil if singular)
- ;; ([inv] '((1 2 3) (2 4 5) (3 5 6)));_([inv] '((2 1) (5 3)))
- (defun [inv] (mat / col piv row res)
- (setq mat (mapcar (function (lambda (x1 x2)
- (append x1 x2)))
- mat
- ([I] (length mat))))
- (while mat
- (setq col (mapcar (function (lambda (x) (abs (car x)))) mat))
- (repeat (vl-position (apply (function max) col) col)
- (setq mat (append (cdr mat) (list (car mat)))))
- (if (equal (setq piv (caar mat)) 0.0 1e-14)
- (setq mat nil
- res nil)
- (setq piv (/ 1.0 (caar mat))
- row (mapcar (function (lambda (x) (* x piv))) (car mat))
- mat (mapcar (function
- (lambda (r / e)
- (setq e (car r))
- (cdr (mapcar (function (lambda (x n)
- (- x (* n e))))
- r
- row))))
- (cdr mat))
- res (cons (cdr row)
- (mapcar
- (function
- (lambda (r / e)
- (setq e (car r))
- (cdr (mapcar (function (lambda (x n)
- (- x (* n e))))
- r
- row))))
- res)))))
- (reverse res))
测试函数
- ;; E.G.
- ;; For Ellipse
- (defun c:test (/ l r p10 a b p11 d40)
- (prompt
- "\nFit Ellipse though Least-square method , the points you selected must be at least 5 !!!")
- (setq l (mapcar (function (lambda (x) (cdr (assoc 10 (entget x)))))
- (vl-remove-if-not
- (function (lambda (x) (eq (type x) 'ENAME)))
- (mapcar 'cadr (ssnamex (ssget '((0 . "POINT")))))))
- l (mapcar (function (lambda (p)
- (trans p 0 1 t)))
- l))
- (if (and (> (length l) 4) (setq r (LS-Pts->Ellipse l)))
- (progn
- (setq p10 (car r)
- p11 (cadr r)
- d40 (caddr r))
- (entmake
- (append
- '(
- (000 . "ELLIPSE")
- (100 . "AcDbEntity")
- (100 . "AcDbEllipse"))
- (list (cons 10 (trans p10 1 0 t))
- (cons 11 (trans p11 1 0))
- (cons 40 d40))
- (list (cons 210 (trans '(0.0 0.0 1.0) 1 0 t)))))))
- (princ)
- )
|