ICP Complex Data Error

I'm working with scans of a single 2D plane of laser data, taken of a 190 degree area. I'm using ICP code (that I found on the matlab forums here: http://www.mathworks.com/matlabcentral/fileexchange/27804-iterative-closest-point) in order to match up the scans and from that, figure out how much the robot has moved. (I changed the original code from that link to fit 2D inputs.) In order to do this, I'm running the ICP in a while loop that updates a plot so that I can see the results.

Everything seems to be working, except that I keep getting this error when I run my code:

??? Error using ==> KDTreeSearcher.knnsearch at 165 Complex data is not allowed.

Error in ==> icp>match_kDtree at 335 [match mindist] = knnsearch(kdOBJ,transpose(p));

Error in ==> icp at 289 [~, mindist] = match_kDtree(q,pt,kdOBJ);

Error in ==> testing_icp_movie at 37 [Ricp Ticp ER t] = icp(M, D, 50,'Matching', 'kDtree', 'Extrapolation',true);

Error in ==> overallprogramming_Try2 at 39 testing_icp_movie;

This leads me to believe that somewhere, it's getting a imaginary number when it tries to compute a square root.

I also get this warning from the ICP code itself:

Warning: Polynomial is badly conditioned. Add points with distinct X values, reduce the degree of the polynomial, or try centering and scaling as described in HELP POLYFIT.

However, the thing that is tripping me up is that when I run the code, with the very same data, variables, and settings, I keep getting the error at different times -- and it hasn't happened in the same place twice. I've gotten this error when the when the while loop has completed 86 times, 102 times, 106 times, 109 times, and 107 times. Why wouldn't this error happen at the same place every time? Any suggestions?

Let me know if there is any other data/code that would help in answering this.

Answer 1

[Walter Roberson]

Perhaps there is some amount of randomization involved? You could experiment with setting the random number generator to the same seed before each run. If you are using a very recent MATLAB version, see rng()

[Jaden]

It does have randomization involved -- I input two sets of data, one already transformed, and one new scan. Then it has this line in the icp code:

% Add noise to model and data

Mtest = Mtest + 0.01*randn(3,width3);

Dtest = Dtest + 0.01*randn(3,width3);

Two follow up questions -- how do I make sure that this randomization doesn't make the data complex? also, why do I need to add noise to my model and data?