@@ -112,11 +112,23 @@ def quat_markley_mean(quats, weights=None):
112112def resample_quat_knn_markley (quat_src , idx , dist , p = 2.0 ):
113113 """
114114 Quaternion resampling using k-NN, IDW weights, and Markley mean.
115- quat_src: (N,4)
116- idx/dist: (M,k)
117- returns (M,4)
115+
116+ quat_src: (N, 4)
117+ idx: (M, k) or (M,)
118+ dist: (M, k) or (M,)
119+ returns: (M, 4)
118120 """
119- Qs = _normalize_quat (quat_src .astype (float , copy = False ))
121+ Qs = _normalize_quat (np .asarray (quat_src , dtype = float ))
122+
123+ idx = np .asarray (idx )
124+ dist = np .asarray (dist )
125+
126+ # Make sure idx/dist are always 2D: (M, k)
127+ if idx .ndim == 1 :
128+ idx = idx [:, None ]
129+ if dist .ndim == 1 :
130+ dist = dist [:, None ]
131+
120132 M , k = idx .shape
121133 out = np .empty ((M , 4 ), dtype = float )
122134
@@ -127,12 +139,12 @@ def resample_quat_knn_markley(quat_src, idx, dist, p=2.0):
127139
128140 nonzero = ~ zero
129141 if np .any (nonzero ):
130- w = _idw_weights (dist [nonzero ], p = p ) # (M', k)
131- neigh = Qs [idx [nonzero ]] # (M', k, 4)
142+ w = _idw_weights (dist [nonzero ], p = p ) # (M', k)
143+ neigh = Qs [idx [nonzero ]] # (M', k, 4)
132144
133- # Sign alignment: align all neighbors to the first neighbor
134- ref = neigh [:, :1 , :] # (M',1, 4)
135- sgn = np .sign (np .sum (neigh * ref , axis = 2 , keepdims = True )) # (M',k,1)
145+ # Align signs to first neighbor
146+ ref = neigh [:, :1 , :] # (M', 1, 4)
147+ sgn = np .sign (np .sum (neigh * ref , axis = 2 , keepdims = True ))
136148 sgn [sgn == 0 ] = 1.0
137149 neigh_aligned = neigh * sgn
138150
0 commit comments