register axis to point cloud
Show older comments
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
Accepted Answer
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);
[~,points]=fun(P);
dist2Lines=vecnorm(points-project(points,r_0_,V_dir))
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
12 Comments
JG
on 27 Dec 2022
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.
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).
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
on 27 Dec 2022
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
on 27 Dec 2022
Matt J
on 27 Dec 2022
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
on 27 Dec 2022
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
on 28 Dec 2022
Matt J
on 28 Dec 2022
OK, but then, what remaining issues are there to discuss? Do you believe the correct solution is not being found?
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
[~,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);
[~,points]=fun(P);
dist2Lines=vecnorm(points-project(points,r_0_,V_dir))
JG
on 28 Dec 2022
Matt J
on 28 Dec 2022
No, I have no further suggestions, other than improve the measurements of r0 and V_dir.
More Answers (2)
Matt J
on 21 Dec 2022
1 vote
7 Comments
JG
on 22 Dec 2022
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?
) 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!
JG
on 22 Dec 2022
Matt J
on 22 Dec 2022
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
on 23 Dec 2022
Here is an implementation using absor() from this FEX download,
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
registrationError
function x=project(x,r_0_,V_dir)
x = r_0_ + sum((x-r_0_).*V_dir).*V_dir;
end
JG
on 27 Dec 2022
This answer would work too but to implement contraints that the points are located where the 
are positive the fmincon answer is more suited.
are positive the fmincon answer is more suited.
Image Analyst
on 22 Dec 2022
0 votes
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.
Categories
Find more on Point Cloud Processing in Help Center and File Exchange
