35 lines
1.3 KiB
Matlab
35 lines
1.3 KiB
Matlab
function v_noisy = addNoise(v_clean,focal_length,pixel_noise)
|
|
|
|
%find good vector in normal plane based on good conditioning
|
|
inPlaneVector1 = zeros(3,1);
|
|
|
|
if v_clean(1,1) > v_clean(2,1) && v_clean(1,1) > v_clean(3,1)
|
|
inPlaneVector1(2,1) = 1.0;
|
|
inPlaneVector1(3,1) = 0.0;
|
|
inPlaneVector1(1,1) = 1.0/v_clean(1,1) * (-v_clean(2,1));
|
|
else
|
|
if v_clean(2,1) > v_clean(3,1) && v_clean(2,1) > v_clean(1,1)
|
|
inPlaneVector1(3,1) = 1.0;
|
|
inPlaneVector1(1,1) = 0.0;
|
|
inPlaneVector1(2,1) = 1.0/v_clean(2,1) * (-v_clean(3,1));
|
|
else
|
|
%v_clean(3,1) > v_clean(1,1) && v_clean(3,1) > v_clean(2,1)
|
|
inPlaneVector1(1,1) = 1.0;
|
|
inPlaneVector1(2,1) = 0.0;
|
|
inPlaneVector1(3,1) = 1.0/v_clean(3,1) * (-v_clean(1,1));
|
|
end
|
|
end
|
|
|
|
%normalize the in-plane vector
|
|
inPlaneVector1 = inPlaneVector1 / norm(inPlaneVector1);
|
|
inPlaneVector2 = cross(v_clean,inPlaneVector1);
|
|
|
|
noiseX = pixel_noise * (rand-0.5)*2.0;% / sqrt(2);
|
|
noiseY = pixel_noise * (rand-0.5)*2.0;% / sqrt(2);
|
|
|
|
v_noisy = focal_length * v_clean + noiseX * inPlaneVector1 + noiseY * inPlaneVector2;
|
|
|
|
v_noisy_norm = norm(v_noisy);
|
|
v_noisy = v_noisy ./ v_noisy_norm;
|
|
|
|
end |