Patent ID: 8849039

Claim:
A method of comparing two object poses, wherein each object pose is expressed in terms of position, orientation and scale with respect to a common coordinate system, the method comprising: calculating a distance between the two object poses, the distance being calculated using the distance function: d sRt ⁡ ( X , Y ) = d s 2 ⁡ ( X , Y ) σ s 2 + d r 2 ⁡ ( X , Y ) σ r 2 + d t 2 ⁡ ( X , Y ) σ t 2 . where X is the object pose of one object and Y is the object pose of the other object, d s ⁡ ( X , Y ) =  log ⁡ ( s ⁡ ( X ) s ⁡ ( Y ) )  , ⁢ d r ⁡ ( X , Y ) =  R ⁡ ( X ) - R ⁡ ( Y )  F , ⁢ d t ⁡ ( X , Y ) =  t ⁡ ( X ) - t ⁡ ( Y )  s ⁡ ( Y ) , s(X) and s(Y) are scalar functions representing the scale of the object poses X and Y respectively, R(X) and R(Y) are matrices expressing the rotation of object poses X and Y respectively, t(X) and t(Y) are vectors expressing the translation of object poses X and Y respectively, and σ s , σ r and σ t are weighting factors for d s , d r and d t respectively.