Skip to content

Commit da54f5b

Browse files
k-okadak-okada
authored
Merge branch 'master' into master
2 parents 2614f75 + f5a24bf commit da54f5b

File tree

2 files changed

+312
-5
lines changed

2 files changed

+312
-5
lines changed

‎irteus/irtpointcloud.l‎

Lines changed: 20 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -545,15 +545,30 @@
545545
(send coords :worldrot)
546546
mat mat)))
547547
ret))
548+
(:set-offset (cds &key (create))
549+
(send self :transform-points cds :create create))
548550
(:convert-to-world (&key (create))
549-
"transform points and normals with self coords. points data should be the same as displayed"
551+
"transform points and normals with self coords. converted points data should be at the same position as displayed"
552+
(send self :move-origin-to (make-coords) :create create))
553+
(:move-origin-to (neworigin &key (create))
554+
"origin of point cloud is moved to neworigin. moved points data should be at the same position as displayed"
550555
(let ((ret
551-
(send self :transform-points (send self :worldcoords) :create create)))
556+
(send self :transform-points
557+
(send (send self :copy-worldcoords) :transform
558+
(send neworigin :inverse-transformation) :world)
559+
:create create)
560+
))
552561
(send ret :reset-coords)
562+
(cond
563+
((and parent (not create)) ;; assocced
564+
(send self :transform
565+
(send (send (send parent :worldcoords) :inverse-transformation)
566+
:transform neworigin))
567+
)
568+
(t
569+
(send ret :transform neworigin)))
553570
(send self :worldcoords)
554571
ret))
555-
(:set-offset (cds &key (create))
556-
(send self :transform-points cds :create create))
557572
;; (:add-normal ())
558573
(:drawnormalmode (&optional mode)
559574
(case mode
@@ -580,7 +595,7 @@
580595
(gl::glDisable gl::GL_LIGHTING)
581596

582597
(gl::glpushmatrix)
583-
(gl::glmultmatrixf (array-entity (transpose (send worldcoords :4x4) gl::*temp-matrix*)))
598+
(gl::glmultmatrixf (array-entity (transpose (send (send self :worldcoords) :4x4) gl::*temp-matrix*)))
584599
;; draw coords
585600
(when (> asize 0.1)
586601
(gl::glLineWidth (float awidth))

‎irteus/test/test-pointcloud.l‎

Lines changed: 292 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,292 @@
1+
(require :unittest "lib/llib/unittest.l")
2+
3+
(init-unit-test)
4+
5+
(defun points-size-check (make-element-func &key (size 10))
6+
(let ((pt-p (instance pointcloud :init
7+
:points (funcall make-element-func)))
8+
(pt-pc (instance pointcloud :init
9+
:points (funcall make-element-func)
10+
:colors (funcall make-element-func)))
11+
(pt-pn (instance pointcloud :init
12+
:points (funcall make-element-func)
13+
:normals (funcall make-element-func)))
14+
(pt-pcn (instance pointcloud :init
15+
:points (funcall make-element-func)
16+
:colors (funcall make-element-func)
17+
:normals (funcall make-element-func))))
18+
(points-size-check-impl pt-p pt-pc pt-pn pt-pcn :size size)
19+
))
20+
21+
(defun points-size-check-impl (pt-p pt-pc pt-pn pt-pcn &key (size 10))
22+
(assert (= (send pt-p :size) size))
23+
(assert (equal (array-dimensions (send pt-p :points)) (list size 3)))
24+
(assert (null (send pt-p :colors)))
25+
(assert (null (send pt-p :normals)))
26+
27+
(assert (= (send pt-pc :size) size))
28+
(assert (equal (array-dimensions (send pt-pc :points)) (list size 3)))
29+
(assert (equal (array-dimensions (send pt-pc :colors)) (list size 3)))
30+
(assert (null (send pt-pc :normals)))
31+
32+
(assert (= (send pt-pn :size) size))
33+
(assert (equal (array-dimensions (send pt-pn :points)) (list size 3)))
34+
(assert (null (send pt-pn :colors)))
35+
(assert (equal (array-dimensions (send pt-pn :normals)) (list size 3)))
36+
37+
(assert (= (send pt-pcn :size) size))
38+
(assert (equal (array-dimensions (send pt-pcn :points)) (list size 3)))
39+
(assert (equal (array-dimensions (send pt-pcn :colors)) (list size 3)))
40+
(assert (equal (array-dimensions (send pt-pcn :normals)) (list size 3)))
41+
42+
(let ((plist (send pt-p :point-list)))
43+
(assert (and (= (length plist) size)
44+
(listp plist))))
45+
(let ((clist (send pt-p :color-list)))
46+
(assert (null clist)))
47+
(let ((nlist (send pt-p :normal-list)))
48+
(assert (null nlist)))
49+
50+
(let ((plist (send pt-pc :point-list)))
51+
(assert (and (= (length plist) size)
52+
(listp plist))))
53+
(let ((clist (send pt-pc :color-list)))
54+
(assert (and (= (length clist) size)
55+
(listp clist))))
56+
(let ((nlist (send pt-pc :normal-list)))
57+
(assert (null nlist)))
58+
59+
(let ((plist (send pt-pn :point-list)))
60+
(assert (and (= (length plist) size)
61+
(listp plist))))
62+
(let ((clist (send pt-pn :color-list)))
63+
(assert (null clist)))
64+
(let ((nlist (send pt-pn :normal-list)))
65+
(assert (and (= (length nlist) size)
66+
(listp nlist))))
67+
68+
(let ((plist (send pt-pcn :point-list)))
69+
(assert (and (= (length plist) size)
70+
(listp plist))))
71+
(let ((clist (send pt-pcn :color-list)))
72+
(assert (and (= (length clist) size)
73+
(listp clist))))
74+
(let ((nlist (send pt-pcn :normal-list)))
75+
(assert (and (= (length nlist) size)
76+
(listp nlist))))
77+
)
78+
79+
(defun filter-check (pt &key (keyword :key) func)
80+
(let ((org-size (send pt :size))
81+
(filtered-pt (send pt :filter keyword func :create t)))
82+
(assert (= (send pt :size) org-size))
83+
(warn ";; filtered ~D points~%" (- org-size (send filtered-pt :size)))
84+
(case keyword
85+
(:key
86+
(dolist (p (send filtered-pt :point-list))
87+
(assert (funcall func p))))
88+
(:ckey
89+
(dolist (c (send filtered-pt :color-list))
90+
(assert (funcall func c))))
91+
(:nkey
92+
(dolist (n (send filtered-pt :normal-list))
93+
(assert (funcall func n))))
94+
(:pckey
95+
(mapc #'(lambda (p c)
96+
(assert (funcall func p c)))
97+
(send filtered-pt :point-list)
98+
(send filtered-pt :color-list)))
99+
(:pnkey
100+
(mapc #'(lambda (p n)
101+
(assert (funcall func p n)))
102+
(send filtered-pt :point-list)
103+
(send filtered-pt :normal-list)))
104+
(:pcnkey
105+
(mapc #'(lambda (p c n)
106+
(assert (funcall func p c n)))
107+
(send filtered-pt :point-list)
108+
(send filtered-pt :color-list)
109+
(send filtered-pt :normal-list)))
110+
(t
111+
(warn ";; unknown keyword ~A~%" keyword)
112+
(assert nil)))
113+
))
114+
115+
(defun filter-check-create ()
116+
(let ((p (make-random-pointcloud :num 20 :with-normal t :with-color t))
117+
pt ret)
118+
(setq pt (instance pointcloud :init
119+
:points (append (list (float-vector 1000 1000 1000)) (send p :point-list))
120+
:colors (append (list (float-vector 1.11 2.22 3.33)) (send p :point-list))
121+
:normals (append (list (float-vector 4.44 5.55 6.66)) (send p :point-list))))
122+
(assert (= (send pt :size) 21))
123+
(setq ret (send pt :filter :key #'(lambda (p) (equal p #f(1000 1000 1000))) :create t))
124+
(assert (= (send pt :size) 21))
125+
(assert (= (send ret :size) 1))
126+
(let ((cl (send ret :color-list)))
127+
(assert (= (length cl) 1))
128+
(equal (car cl) (float-vector 1.11 2.22 3.33)))
129+
(let ((nl (send ret :normal-list)))
130+
(assert (= (length nl) 1))
131+
(equal (car nl) (float-vector 4.44 5.55 6.66)))
132+
133+
(send pt :filter :key #'(lambda (p) (equal p #f(1000 1000 1000))))
134+
(setq ret pt)
135+
(assert (= (send ret :size) 1))
136+
(let ((cl (send ret :color-list)))
137+
(assert (= (length cl) 1))
138+
(equal (car cl) (float-vector 1.11 2.22 3.33)))
139+
(let ((nl (send ret :normal-list)))
140+
(assert (= (length nl) 1))
141+
(equal (car nl) (float-vector 4.44 5.55 6.66)))
142+
))
143+
144+
(defun convert-to-world-check ()
145+
(let* ((*origin-p* (float-vector 500 1000 1500))
146+
(*point*
147+
(instance pointcloud :init :points (list *origin-p*) :point-size 10))
148+
(*bcoords* (make-coords :pos (float-vector 100 200 300) :rpy (list 0.1 0.2 0.3)))
149+
(*base* (make-cascoords :pos (float-vector 200 -100 -150) :rpy (list 0.2 -0.2 0.1)))
150+
)
151+
(send *base* :assoc *point*)
152+
(let ((p (send *point* :convert-to-world :create t)))
153+
(assert (eps-v= (car (send *point* :point-list)) *origin-p* 1e-8)) ;; original does not move
154+
(assert (eps-v= (car (send p :point-list)) *origin-p* 1e-8))
155+
)
156+
157+
(send *base* :transform *bcoords*)
158+
(let ((p (send *point* :convert-to-world :create t)))
159+
(assert (eps-v= (car (send *point* :point-list)) *origin-p* 1e-8)) ;; original does not move
160+
(assert (eps-v=
161+
(car (send p :point-list))
162+
(send (send *point* :worldcoords) :transform-vector (car (send *point* :point-list)))
163+
1e-8))
164+
)
165+
(let ((pt (send (send *point* :worldcoords) :transform-vector (car (send *point* :point-list)))))
166+
(send *point* :convert-to-world :create nil)
167+
(assert (eps-v=
168+
(car (send *point* :point-list))
169+
(send (send *point* :worldcoords) :transform-vector (car (send *point* :point-list)))
170+
1e-8))
171+
(assert (eps-v=
172+
(car (send *point* :point-list)) ;; original moved
173+
pt
174+
1e-8))
175+
(assert (eps-v= (send *point* :worldpos) (float-vector 0 0 0) 1e-8))
176+
(assert (eps-v= (array-entity (send *point* :worldrot))
177+
(float-vector 1 0 0 0 1 0 0 0 1) 1e-8))
178+
)
179+
))
180+
181+
(defun move-origin-to-check ()
182+
(let* ((*origin-p* (float-vector 500 1000 1500))
183+
(*point*
184+
(instance pointcloud :init :points (list *origin-p*) :point-size 10))
185+
(*bcoords* (make-coords :pos #f(100 200 300) :rpy (list 0.1 0.2 0.3)))
186+
(*ccoords* (make-coords :pos #f(0 0 1000) :rpy (list 0.2 -0.2 0.1)))
187+
)
188+
(when (member :move-origin-to (send *point* :methods))
189+
(send *point* :move-origin-to *bcoords*)
190+
(assert
191+
(eps-v=
192+
(send (send *point* :worldcoords) :transform-vector (car (send *point* :point-list)))
193+
*origin-p*
194+
1e-8))
195+
(assert
196+
(eps-v=
197+
(send *bcoords* :transform-vector (car (send *point* :point-list)))
198+
(send (send *point* :worldcoords) :transform-vector (car (send *point* :point-list)))
199+
1e-8))
200+
201+
(send *point* :move-origin-to *ccoords*)
202+
(assert
203+
(eps-v=
204+
(send (send *point* :worldcoords) :transform-vector (car (send *point* :point-list)))
205+
*origin-p*
206+
1e-8))
207+
(assert
208+
(eps-v=
209+
(send *ccoords* :transform-vector (car (send *point* :point-list)))
210+
(send (send *point* :worldcoords) :transform-vector (car (send *point* :point-list)))
211+
1e-8))
212+
213+
(send *point* :move-origin-to (make-coords))
214+
(assert
215+
(eps-v=
216+
(send (send *point* :worldcoords) :transform-vector (car (send *point* :point-list)))
217+
*origin-p*
218+
1e-8))
219+
220+
(setq *point*
221+
(instance pointcloud :init :points (list *origin-p*) :point-size 10))
222+
(let ((base (make-cascoords :pos (float-vector -300 -200 -100) :rpy (list 0.3 -0.2 0.1))))
223+
(send base :assoc *point*)
224+
(send base :transform
225+
(make-coords :pos (float-vector 400 -800 1200) :rpy (list 0.4 0.3 0.2)))
226+
(assert
227+
(eps-v=
228+
(send (send *point* :worldcoords) :transform-vector (car (send *point* :point-list)))
229+
(float-vector 792.732209445 539.4908242525 2397.9448709116)
230+
1e-8))
231+
232+
(send *point* :move-origin-to *ccoords*)
233+
(assert
234+
(eps-v=
235+
(send (send *point* :worldcoords) :transform-vector (car (send *point* :point-list)))
236+
(float-vector 792.732209445 539.4908242525 2397.9448709116)
237+
1e-8))
238+
(assert
239+
(eps-v= (send *point* :worldpos) (send *ccoords* :pos) 1e-8))
240+
(assert
241+
(eps-v=
242+
(array-entity (send *point* :worldrot))
243+
(array-entity (send *ccoords* :rot)) 1e-8))
244+
)
245+
)
246+
))
247+
#|
248+
(send pt :transform-points cds :create t)
249+
(send pt :append (list ...))
250+
|#
251+
252+
(deftest test-pointcloud-create
253+
;;
254+
(points-size-check #'(lambda () (make-matrix 10 3)))
255+
;;
256+
(points-size-check #'(lambda () (let (l) (dotimes (i 10) (push (float-vector 0 0 0) l)) l)))
257+
;;
258+
(points-size-check-impl
259+
(make-random-pointcloud :num 100)
260+
(make-random-pointcloud :num 100 :with-color t)
261+
(make-random-pointcloud :num 100 :with-normal t)
262+
(make-random-pointcloud :num 100 :with-color t :with-normal t)
263+
:size 100)
264+
)
265+
266+
(deftest test-pointcloud-filter
267+
;;
268+
(filter-check (make-random-pointcloud :num 40)
269+
:keyword :key :func #'(lambda (p) (> (elt p 0) 0)))
270+
(filter-check (make-random-pointcloud :num 40 :with-color t)
271+
:keyword :ckey :func #'(lambda (c) (> (elt c 0) 0)))
272+
(filter-check (make-random-pointcloud :num 40 :with-normal t)
273+
:keyword :nkey :func #'(lambda (n) (> (elt n 0) 0)))
274+
(filter-check (make-random-pointcloud :num 40 :with-color t)
275+
:keyword :pckey :func #'(lambda (p c) (> (elt c 0) 0)))
276+
(filter-check (make-random-pointcloud :num 40 :with-normal t)
277+
:keyword :pnkey :func #'(lambda (p n) (> (elt n 0) 0)))
278+
(filter-check (make-random-pointcloud :num 40 :with-color t :with-normal t)
279+
:keyword :pcnkey :func #'(lambda (p c n) (> (elt p 0) 0)))
280+
;;
281+
(filter-check-create)
282+
)
283+
284+
(deftest test-pointcloud-convert
285+
;;
286+
(convert-to-world-check)
287+
;;
288+
(move-origin-to-check)
289+
)
290+
291+
(run-all-tests)
292+
(exit)

0 commit comments

Comments
 (0)