register axis to point cloud

Hello everyone,

I try to register a set of 3 lines to a set of 3 points. The Coordinate System of both sets are not the same. So I need to find the rotation and translation of the lines so they pass through the points. Does anyone know a good method to do so?

little bit of Context:

the lines are the axis of a drilling wholes obtained by CT Scans and the points are marker obtained from a Motion Capture System. The markers were on the screw inserted into the drilling wholes. I need to match them in order that I can register the CT Data to the MotionCapture System

edit:

Here an example:

MoCap points:

  MoCap_points = [243.9669  203.8436  215.5850
                  104.2774  117.8749   85.7087
                 -113.6220 -106.1354 -102.1162];

Axis line in form:

r_0_ = 1.0e+03 *[0.6269    0.6139    0.6055
                 -1.6006   -1.6178   -1.6009
                 -0.3004   -0.2963   -0.3010];
V_dir =     [0.6183    0.0112   -0.2819
            0.2555   -0.3098    0.3367
            0.7432    0.9507    0.8984];
           

Each Colum is a different vectors.

Thanks for any suggestion,

JG

[Matt J]

Is the correspondence between lines and points known? I.e., are we sure that the point given by the i-th column of MoCap_points and the line described by the i-th column of r_0_, V_dir form an intersecting pair?

[Matt J]

Axis line in form:

Is there any information you can give on the s_i ranges? These lines are finite 3D line segments, I assume, so s_i should range over some finite interval [sLower,sUpper].

[JG]

yes they are corresponding. The range of s_i shold be between 0 and 50.

Answer 1

[Matt J]

Here is an optimization implemented with fmincon. It achieved a lower registration error dist2Lines than my other answer, though, it's hard to say whether the naive initialization P0=eye(3,4) will work as well on other data sets.

MoCap_points = [243.9669  203.8436  215.5850
                  104.2774  117.8749   85.7087
                 -113.6220 -106.1354 -102.1162];
  
  r_0_ = 1.0e+03 *[0.6269    0.6139    0.6055
                 -1.6006   -1.6178   -1.6009
                 -0.3004   -0.2963   -0.3010];
  V_dir =     [0.6183    0.0112   -0.2819
            0.2555   -0.3098    0.3367
            0.7432    0.9507    0.8984];    V_dir=normalize(V_dir,1,'n');

  
fun=@(P) objFcn(P,r_0_,V_dir, MoCap_points);

P0=eye(3,4);

[P,fval]=fmincon(fun,P0,[],[],[],[],[],[],@nlcFcn);

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 1.108598e-21.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 1.108598e-21.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 1.584718e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 1.584718e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 3.172984e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 3.172984e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 3.172984e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 3.172984e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 3.172984e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 3.172984e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 3.172984e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 3.172984e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 3.172984e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 3.172984e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 3.172984e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 3.172984e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 3.172984e-18.

Warning: Matrix is close to singular or badly scaled. Results may be inaccurate. RCOND = 3.172984e-18.

Local minimum possible. Constraints satisfied.

fmincon stopped because the size of the current step is less than
the value of the step size tolerance and constraints are 
satisfied to within the value of the constraint tolerance.

[~,points]=fun(P);

dist2Lines=vecnorm(points-project(points,r_0_,V_dir))

dist2Lines = 1×3

1.0e-03 * 0.1395 0.0835 0.0937

function [fval,x]=objFcn(P,r_0_,V_dir, MoCap_points)

 R=P(:,1:3); t=P(:,end);

 x=R*MoCap_points+t;

 fval=norm(  x-project( x,r_0_,V_dir)  ,'fro').^2;


end


function [c,ceq]=nlcFcn(P)

 R=P(:,1:3); 

 ceq=R*R'-eye(3);
 c=1-det(R);

end


function x=project(x,r_0_,V_dir)

    x = r_0_ + sum((x-r_0_).*V_dir).*V_dir;

end

[JG]

Thanks a lot.

I had to adjust the nonlinear contraint so that the parameters of the vector of the hole axis in order that it stayed positive.

function [c,ceq]=nlcFcn(P, MoCap_points, r_0_, V_dir)

 R = P(:,1:3); 
 t = P(:,end);
 ceq = R*R'-eye(3);
 c = 1-det(R);
 x = R*MoCap_points + t;
 c(2:4) = -sum((x-r_0_).*V_dir)';
end

and consequently the fmincon line:

[P,fval]=fmincon(fun,P0,[],[],[],[],[],[],@(P)nlcFcn(P, MoCap_points,  r_0_, V_dir));

that forces the Mocap points (blue) to be outside of the scanned body (yellowish) (see below). However, I expected the points closer to the black points. I suppose, that the axis of the hole are not measured precise enough.

Let me know if you have any thoughts about this. I let this threat open until you answer. I'll accept if you have no further suggestion (since I think you have answered my initial question well).

[Matt J]

Looks good to me. If you know the MoCap points are supposed to be close to the black points, it would be advisable to use absor to find the rototranslation that brings them there and use that rototranslation to initialize fmincon. That might also make it possible to omit the new nonlinear constraints that you've added. Intializing in the vicinity of a solution with positive s might be enough, in other words.

[JG]

Well the black points are a very rough estimate (I drawed them by hand). The function absor yielded points inside the body... So you would suggest to do this iterativly. Meaning, I calcultate roughly the position (blue points), amend the position that they are roughly close to the black points and use absor and then fmincon again?

[Matt J]

So you would suggest to do this iterativly.

No, I was suggesting that you simply register the MoCap points to the black points. That is a non-iterative, one-step use of absor. You would then use that registration result to generate your initial guess P0 to fmincon. This P0 should give s>0, because the the black points have s>0.

[JG]

alright I see. I have tried this I am still not happy with the result. The better the guess of the black points gives better results however, I was hoping for a preciser result.

Do you have another suggestion besides improving the initial guess (black points)?

[Matt J]

I'm not sure how you are evaluating the result. If you believe the result should bring you pretty close to the black points, you need to assess that hypothesis by looking at the error output from absor when you do that initial registration. You haven't given me the coordinates of the black points, but if they simply corresponding to s=50, the initial registration I find below is pretty poor, making your hypothesis suspect:

[reg,~,err]=absor(MoCap_points, r_0_ + 50*V_dir ); err

err =

struct with fields:

errlsq: 25.2735

errmax: 16.4152

[Matt J]

Additionally, both the home-brewed iterative method in my first answer and the fmincon approach are giving the same result. This strengthens my confidence that both methods are converging well to the correct optimum, and therefore that the performance you are seeing is the best you are going to squeeze out of this data.

[JG]

I was affraid of this. I do not know the exact location of the black points. It is simply a comparision of the static images before cunducting the MoCap measurments. The blue points should not be on the spherical surface but rather roughly 20 mm away, but it can vary over the different specimen.

[Matt J]

OK, but then, what remaining issues are there to discuss? Do you believe the correct solution is not being found?

[Matt J]

Another way to verify that the global minimum is being found is by an exhaustive sweep over all combination of s_i values. The example below does this in 1 mm increments of s. The same solution is before is found, however:

fun=@(P) objFcn(P,r_0_,V_dir, MoCap_points);
nlc=@(P) nlcFcn(P,r_0_,V_dir, MoCap_points);

[T1,T2,T3]=ndgrid(0:50);
fval=nan(size(T1));

tic;
 N=numel(T1);
 for i=1:N

     s=[T1(i),T2(i),T3(i)]; 
     [~,~,Error]=absor(MoCap_points, r_0_+s.*V_dir );

     fval(i)=Error.errmax;

 end
toc

Elapsed time is 6.220321 seconds.

    [~,i]=min(fval(:));
     s=[T1(i),T2(i),T3(i)]; 
     [reg,points,Error]=absor(MoCap_points, r_0_+s.*V_dir );


P0=reg.M(1:3,:);

opts=optimoptions('fmincon', 'Algorithm','interior-point',...
                  'StepTol',1e-12,'FunctionTol',1e-12,'OptimalityTol',1e-12);
[P,fval,ef]=fmincon(fun,P0,[],[],[],[],[],[],nlc,opts);

Local minimum possible. Constraints satisfied.

fmincon stopped because the size of the current step is less than
the value of the step size tolerance and constraints are 
satisfied to within the value of the constraint tolerance.

[~,points]=fun(P);
dist2Lines=vecnorm(points-project(points,r_0_,V_dir))

dist2Lines = 1×3

1.2354 1.2625 1.4517

[JG]

Yes I belive the solution is no precise enough, My end goal is to relate the joint centre of the CT data to the MoCap Data, but I supose there is nothig left to squeeze out of the data.

I would accept the answer if you have no further sugggestions

Thanks alot for your effort.

[Matt J]

No, I have no further suggestions, other than improve the measurements of r0 and V_dir.

Answer 2

[Matt J]

This paper does something quite similar.

https://www.researchgate.net/publication/224380689_Compensating_for_head_motion_in_slowly-rotating_cone_beam_CT_systems_with_optimization_transfer_based_motion_estimation

[JG]

I must addmit I do not yet understand the paper well enough to transfer it to my problem. Maybe you can clear something up for me. Do the Line of Response () all have to meet at one point in order that the algorithm works?

In my case the axis of the drilling hole do not necessarly meet.

Thank you!

[Matt J]

No, they don't have to meet, but the assumption is that they are not parallel. If they were all parallel, the translation is ill-defined, because any movement in the parallel direction of the lines would not upset the intersections.

[JG]

hmm I think it does not work in my case. I only have one time instance of the hole axis whereas in the paper the x-ray axis repeated in time. My case would be under-deterministic.

Do you have another suggestion or is there a way to amend the algorithm to my case?

Thanks,

JG

[Matt J]

I think it should be applicable to your case, but it might be easiest to demonstrate that if you provided some data, in particular, the 3D location of the points and some mathematical description of the lines, e.g., the 3D coordinates of their end points, if they are line segments.

[JG]

I am curious to see how it works. I have eddited the post above with an example.

[Matt J]

Here is an implementation using absor() from this FEX download,

https://www.mathworks.com/matlabcentral/fileexchange/26186-absolute-orientation-horn-s-method

The registration brings the points into an intersection with the lines to within a worst error of 1.45. I don't know what kind of registration error magnitudes you were expecting. Possibly, one could do better using fmincon().

  MoCap_points = [243.9669  203.8436  215.5850
                  104.2774  117.8749   85.7087
                 -113.6220 -106.1354 -102.1162];
  
  r_0_ = 1.0e+03 *[0.6269    0.6139    0.6055
                 -1.6006   -1.6178   -1.6009
                 -0.3004   -0.2963   -0.3010];
  V_dir =     [0.6183    0.0112   -0.2819
            0.2555   -0.3098    0.3367
            0.7432    0.9507    0.8984];    V_dir=normalize(V_dir,1,'n');
  
  
  
points=MoCap_points;

Niter=500;
 for i=1:Niter

     [registrationParams,points,registrationError]=absor(MoCap_points, project(points,r_0_,V_dir) );

 end



registrationParams

registrationParams = struct with fields:
    R: [3×3 double]
    t: [3×1 double]
    s: 1
    M: [4×4 double]
    q: [4×1 double]

registrationError

registrationError = struct with fields:
    errlsq: 2.2864
    errmax: 1.4518

function x=project(x,r_0_,V_dir)

    x = r_0_ + sum((x-r_0_).*V_dir).*V_dir;

end

[JG]

This answer would work too but to implement contraints that the points are located where the are positive the fmincon answer is more suited.

Answer 3

[Image Analyst]

I don't know if it's as sophisticated as your paper, but you can use the built-in imregister to align each slice to the prior one, or the first one.

Or you can find the centroids of the holes with regionprops and then use imtranslate to shift a slice to the desired position.

[JG]

My problem is not find the axis it self. I try to find a best fit from the MoCap data to the Axis extracted from the CT. I have added an example in the post above