I'm using minimize() function on the loop, trying to find rotation-translation matrix between pointcloud and camera pixels. As a parameters I'm giving 6 points coordinates in camera frame (uvs) and corresponding xyz (points) and calculate error:
result = minimize( costFunction, settings[ 'initialTransform' ], args = ( 1.0, False ), bounds = settings[ 'bounds' ], method = 'SLSQP', options = { 'disp': True, 'maxiter': 1000 } )
while not result.success or result.fun > 50:
for i in range( 0, len( settings[ 'initialTransform' ] ) ):
settings[ 'initialTransform' ][ i ] = random.uniform( settings[ 'bounds' ][ i ][ 0 ], settings[ 'bounds' ][ i ][ 1 ] )
result = minimize( costFunction, settings[ 'initialTransform' ], args = ( 1.0, False ), bounds = settings[ 'bounds' ], method = 'SLSQP', options = { 'disp': True, 'maxiter': 1000 } )
It does work, however it looks for me like calculations stuck in the infinite loop - it just can't find an optimal solution. Here is a costFunction implementation:
def costFunction( x, sign = 1.0, debug = False ):
global cameraModel, settings
parameters = x
translation = [ parameters[ 0 ], parameters[ 1 ], parameters[ 2 ], 1.0 ]
rotationMatrix = tf.transformations.euler_matrix( parameters[ 5 ], parameters[ 4 ], parameters[ 3 ] )
rotationMatrix[ :, 3 ] = translation
error = 0
for i in range( 0, len( settings[ 'points' ] ) ):
point = settings[ 'points' ][ i ]
expectedUV = settings[ 'uvs' ][ i ]
rotatedPoint = rotationMatrix.dot( point )
uv = cameraModel.project3dToPixel( rotatedPoint )
diff = np.array( uv ) - np.array( expectedUV )
error = error + math.sqrt( np.sum( diff ** 2 ) )
return sign * error
So I'm wondering if I give more than 6 point will it help or should I increase threshold for error? Appreciate any hints and algorithm improvements.