Lisp通用最小二乘法

高山流水 · 发表于 2014-3-12 22:22:36

马上注册，结交更多好友，享用更多功能，让你轻松玩转社区。

您需要登录才可以下载或查看，没有账号？立即注册

×

通用最小二乘法在代码和原理上相对比较简单，麻烦的是每次应用都得把高次元参数转换成低次参数，这里写出我的写法和大家探讨，希望能有更多的意见一起完善它。

(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))

;; 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))

应用举例，最小二乘法拟合椭圆

;;;-------------------------------------------------------------
(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)))))
))
;;;-------------------------------------------------------------
;; 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)
)

;;;------------------------------------------------------------- 
(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))))) 
    )) 
;;;------------------------------------------------------------- 
;; 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) 
  )

最小二乘法拟合圆

(defun LS-Pts->Circle (lst nchsep / arg_mat res_vec
ps pe xs ys xe ye xc yc
a b c d res)
;; Function:
;; Pointset return Circle by Least Square method
;; args :
;; lst -- Pointset , > 3 points
;; nchsep -- Is Not change the start and end point ? T or Nil
;; by GSLS(SS)
;; 2013-11-16
;; 2014-01-01 Edited , Add constant the startpoint and endpoint mode .
(if (> (length lst) 2)
(if nchsep
(progn
(setq ps (car lst)
xs (car ps)
ys (cadr ps)
pe (last lst)
xe (car pe)
ye (cadr pe))
(mapcar (function (lambda (p / x y)
(setq x (car p)
y (cadr p))
(setq arg_mat (cons (list (- (+ x x) xs xe)
(- (+ y y) ys ye))
arg_mat)
res_vec (cons
(- (+ (* x x) (* y y))
(/2 (+ (* xs xs)
(* ys ys)
(* xe xe)
(* ye ye))))
res_vec))))
(butlast (cdr lst)))
(setq res (solve-odeq arg_mat res_vec))
(if (and (cadr res)
(setq xc (car res)
yc (cadr res))
(setq d (/2 (+ (distance (list xc yc) ps)
(distance (list xc yc) pe))))
(setq c
(car
(vl-sort
(c_int_c ps d pe d)
(function (lambda (a b)
(< (distance a (list xc yc))
(distance b (list xc yc)))))))))
(list c d)))
(progn
(mapcar (function (lambda (p / x y)
(setq x (car p)
y (cadr p))
(setq arg_mat (cons (list x y 1.) arg_mat)
res_vec (cons (/2 (+ (* x x) (* y y))) res_vec))))
lst)
(setq res (solve-odeq arg_mat res_vec))
(if (and (setq c (caddr res))
(setq a (car res)
b (cadr res))
(> (setq d (+ (* a a) (* b b) c c)) 0))
(list (list a b) (sqrt d)))))))
;; For Circle
(defun c:Test (/ l r p10 d40)
(prompt "\nFit Circle though Least-square method , the points you selected must be at least 3 !!!")
(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) 2)(setq r (LS-Pts->Circle l nil)))
(progn
(setq p10 (car r)
d40 (cadr r))
(entmakex
(list (cons 0 "CIRCLE")
(cons 10 (trans p10 1 0 t))
(cons 40 d40)
(cons 210 (trans (quote(0.0 0.0 1.0)) 1 0 t))))
))
(princ)
)

(defun LS-Pts->Circle  (lst   nchsep   / arg_mat     res_vec 
   ps    pe    xs   ys xe    ye    xc   yc 
   a     b     c   d res) 
  ;; Function: 
  ;; Pointset return Circle by Least Square method 
  ;; args : 
  ;;     lst --  Pointset , > 3 points 
  ;;     nchsep -- Is Not change the start and end point ? T or Nil 
  ;; by GSLS(SS) 
  ;; 2013-11-16 
  ;; 2014-01-01 Edited , Add constant the startpoint and endpoint mode . 
  (if (> (length lst) 2) 
    (if nchsep 
      (progn 
 (setq ps (car lst) 
       xs (car ps) 
       ys (cadr ps) 
       pe (last lst) 
       xe (car pe) 
       ye (cadr pe)) 
 (mapcar (function (lambda (p / x y) 
       (setq x (car p) 
      y (cadr p)) 
       (setq arg_mat (cons (list (- (+ x x) xs xe) 
            (- (+ y y) ys ye)) 
      arg_mat) 
      res_vec (cons 
         (- (+ (* x x) (* y y)) 
            (/2 (+ (* xs xs) 
            (* ys ys) 
            (* xe xe) 
            (* ye ye)))) 
         res_vec)))) 
  (butlast (cdr lst))) 
 (setq res (solve-odeq arg_mat res_vec)) 
 (if (and (cadr res) 
   (setq xc (car res) 
    yc (cadr res)) 
   (setq d (/2 (+ (distance (list xc yc) ps) 
      (distance (list xc yc) pe)))) 
   (setq c 
     (car 
       (vl-sort 
         (c_int_c ps d pe d) 
         (function (lambda (a b) 
       (< (distance a (list xc yc)) 
          (distance b (list xc yc))))))))) 
   (list c d))) 
      (progn 
 (mapcar (function (lambda (p / x y) 
       (setq x (car p) 
      y (cadr p)) 
       (setq arg_mat (cons (list x y 1.) arg_mat) 
      res_vec (cons (/2 (+ (* x x) (* y y))) res_vec)))) 
  lst) 
 (setq res (solve-odeq arg_mat res_vec)) 
 (if (and (setq c (caddr res)) 
   (setq a (car res) 
         b (cadr res)) 
   (> (setq d (+ (* a a) (* b b) c c)) 0)) 
   (list (list a b) (sqrt d))))))) 
;; For Circle 
(defun c:Test  (/ l r p10 d40) 
  (prompt "\nFit Circle though Least-square method , the points you selected must be at least 3 !!!") 
  (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) 2)(setq r (LS-Pts->Circle l nil)))    
    (progn 
      (setq p10 (car r)       
     d40 (cadr r)) 
      (entmakex 
 (list (cons 0 "CIRCLE") 
       (cons 10 (trans p10 1 0 t)) 
       (cons 40 d40) 
       (cons 210 (trans (quote(0.0 0.0 1.0)) 1 0 t)))) 
  ))   
  (princ) 
  )

拉契 · 发表于 2014-3-12 22:27:33

先占个位置顺便顶起

/db_自贡黄明儒_ · 发表于 2014-3-13 09:42:48

长老很good,搞些基础研究，赞！！！

dwg001 · 发表于 2014-3-13 18:13:49

相当不错的数学基础。

skg123 · 发表于 2017-2-24 15:12:52

拟合圆的方程中 lst 是点列表，那么 nchsep 是什么？水平有限，请指点

账号		自动登录	找回密码
密码			立即注册

[研讨] Lisp通用最小二乘法

马上注册，结交更多好友，享用更多功能，让你轻松玩转社区。

相关帖子