Without being able to run the code I suspect that the index that is exceeding the matrix dimension is not 'i' as an index to fgx or fgy, but the result of fgx(i) or fgy(i) exceeds the dimensions of 'im' when you try to index into it using these.
What is the size of 'im' and what are the values of fgx(129) and fgy(129)?