Creata function per vedere le feature

Observations_Generation.m

@@ -12,10 +12,45 @@ clc
     restoredefaultpath
     addpath('../useful_functions/');
     addpath(genpath('../mice/'));
+    addpath(genpath('../computer-vision/'));
+    addpath(genpath('../paper/'));
     addpath(genpath('../generic_kernels/'));
     addpath(genpath('../paper/MMX_Fcn_CovarianceAnalyses/'));
     addpath(genpath('../paper/MMX_Product/MMX_BSP_Files_GravLib/'));
     MMX_InitializeSPICE
     cspice_furnsh(which('mar097.bsp'));
     cspice_furnsh(which('MARPHOFRZ.txt'));
+    cspice_furnsh(which('MMX_QSO_049_2x2_826891269_828619269.bsp'));
+    cspice_furnsh(which('Phobos_826891269_828619269.bsp'));
+    
 
+    %   Time of the analysis
+    data        = '2026-03-16 13:00:00 (UTC)';
+    data        = cspice_str2et(data);
+    
+%   Model parameters
+    [par, units] = MMX_DefineNewModelParametersAndUnits;
+
+%   Covariance analysis parameters
+    par.et0      = data;
+    [par, units] = New_MMX_CovarianceAnalysisParameters(par, units);
+    
+    cam.f       = 15;                       %[mm]
+    cam.nPixX   = 3296;                     %[n. pixel]
+    cam.nPixY   = 2472;                     %[n. pixel]
+    cam.Res     = 100;
+    cam.SixeX   = 36;                       %[mm]
+    cam.SixeY   = 24;                       %[mm]
+    cam.pixARx  = 1;
+    cam.pixARy  = 1;
+    par.camera_intrinsic = IntrinsicParameters(cam);
+    par.FOV = deg2rad(30);
+
+    %   Positin of my target bodies at that date
+    Sun     = cspice_spkezr('10', data, 'J2000', 'none', 'MARS');
+    MMX     = cspice_spkezr('-34', data, 'J2000', 'none', 'MARS');
+    Phobos  = cspice_spkezr('-401', data, 'J2000', 'none', 'MARS');
+    Phi     = deg2rad(0);
+
+    [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi,...
+        'Principal_Phobos_Ogni5.mat', par, units);

ProjMatrix.m

+function [M_int, M_ext, PM] = ProjMatrix(t, pars, units)
+
+%   Camera intrinsic matrix
+    K     = pars.camera_intrinsic;
+
+
+    
+        
+%   Camera's fixed reference frame has the Z versor along the radial
+%   direction, and the X and Y on the camera plane.
+    k = t/norm(t);
+    v = [0; 0; 1];
+    r = cross(v,k)/norm(cross(v,k));
+    v = cross(k,r)/norm(cross(k,r));
+
+    R_bCam2World    = [r, v, k];
+    R_World2bCam    = R_bCam2World';
+
+%   Target's position vector WRT Camera fixed reference frame 
+    T_bCam      = -R_World2bCam*t;
+
+%   Target's position WRT image plane
+    R_bCam2CV   = [1, 0, 0; 0, -1, 0; 0, 0, -1];
+    T_CV        = R_bCam2CV*T_bCam;
+
+%   Una rotazione dell'asteroide corrisponde ad una apparente rotazione 
+%   inversa della camera 
+    R_World2Ast     = eye(3);
+    R_FicbCam2World = R_World2Ast';
+    R_World2FicbCam = R_FicbCam2World';
+    
+%   Ma a questa rotazione va aggiunta la rotazione definita in
+%   precendenza nella definizione del camera fixed reference frame 
+    R_FicWorld2CV   = R_World2bCam*R_World2FicbCam;
+
+%   E la rotazione per portarla nell'image plane
+    R_World2CV      = R_bCam2CV*R_FicWorld2CV;
+
+%   The asteroid instead is fixed
+    R_Ast2World     = eye(3);
+
+%   The rotation from the asteroid to the fake image plane is at the end  
+    R_FicAst2CV     = R_Ast2World*R_World2CV;
+
+%   Projection matrix
+    M_int   = [K', zeros(3,1)];
+    M_ext   = [R_FicAst2CV, T_CV; 0, 0, 0, 1];
+    PM      = M_int*M_ext;
+
+    
+end
\ No newline at end of file

visible_features.asv

-function Y = visible_features(data, file_features, pars, units)
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-%
-% Y = visible_features(data, file_features)
-%
-% Identify the visible features and build the observable for analysis
-%
-% Input:
-%     
-% data          Date of the observation
-% file_features File including the position of the features in the body
-%               fixed reference frame
-% 
-% Output:
-% 
-% Y             Vector of features' pixel coordinates in the picture 
-%
-% Author: E.Ciccarelli
-%
-%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
-
-%   Features defined in the Phobos-fixed reference frame
-    load(file_features);
-    points = Point_Cloud';
-
-%   Positin of my target bodies at that date
-    Sun     = cspice_spkezr('10', data, 'J2000', 'none', 'MARS');
-    MMX     = cspice_spkezr('-34', data, 'J2000', 'none', 'MARS');
-    Phobos  = cspice_spkezr('-401', data, 'J2000', 'none', 'MARS');
-    
-%   Rotation from J200 to Phobos radial reference frame
-
-    i       = Phobos(1:3)/norm(Phobos(1:3));
-    v       = Phobos(4:6)/norm(Phobos(4:6));
-    k       = cross(i, v)/norm(cross(i, v));
-    j       = cross(k, i)/norm(cross(k, i));
-    
-    R2rot   = [i, j, k]';
-
-%   Rotation from Phobos radial reference frame to Phobos-fixed reference frame
-
-    RLibration   = [cos(Phi), sin(Phi), 0; -sin(Phi), cos(Phi), 0; 0, 0, 1];
-
-%   Light rays direction in the Phobos-fixed reference frame
-    r_PhSun  = RLibration*R2rot*(Sun(1:3) - Phobos(1:3))/norm(Sun(1:3) - Phobos(1:3));
-
-%   Scan on the features to distinguish those in light
-    point_in_light = [];
-
-    for n = 1:size(points,2)
-%       If the dot product of the incoming light ray and the feature
-%       position vector in the Phobos-fixed ref frame is less than zero,
-%       then it is in light
-        if dot(r_PhSun, points(:,n))<0
-            point_in_light = [points(:,n), point_in_light];
-        end
-    end
-
-
-%   Rotation matrix to the camera fixed frame
-    r_sb    = MMX(1:3) - Phobos(1:3);
-    R_sb    = norm(r_sb);
-    
-    i       = r_sb/R_sb;
-    v       = [0; 0; 1];
-    j       = cross(v, i)/norm(cross(v, i));
-    k       = cross(i, j)/norm(cross(i, j));
-    
-    T       = [i, j, k]';
-
-
-end
\ No newline at end of file

visible_features.m

-function Y = visible_features(data, file_features, pars, units)
+function [Y_LOS, Y_pix] = visible_features(MMX, Phobos, Sun, Phi, file_features, pars, units)
 %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
 %
 % Y = visible_features(data, file_features)
@@ -21,23 +21,20 @@ function Y = visible_features(data, file_features, pars, units)
 
 %   Features defined in the Phobos-fixed reference frame
     load(file_features);
-    points = Point_Cloud';
+    R_ini   = [0, -1, 0; 1, 0, 0; 0, 0, 1];
+    points  = R_ini*Point_Cloud';
+
 
-%   Positin of my target bodies at that date
-    Sun     = cspice_spkezr('10', data, 'J2000', 'none', 'MARS');
-    MMX     = cspice_spkezr('-34', data, 'J2000', 'none', 'MARS');
-    Phobos  = cspice_spkezr('-401', data, 'J2000', 'none', 'MARS');
-    
 %%  Identification of the features that are in light
 
 %   Rotation from J200 to Phobos radial reference frame
 
-    i       = Phobos(1:3)/norm(Phobos(1:3));
+    i_feat       = Phobos(1:3)/norm(Phobos(1:3));
     v       = Phobos(4:6)/norm(Phobos(4:6));
-    k       = cross(i, v)/norm(cross(i, v));
-    j       = cross(k, i)/norm(cross(k, i));
+    k       = cross(i_feat, v)/norm(cross(i_feat, v));
+    j       = cross(k, i_feat)/norm(cross(k, i_feat));
     
-    R2rot   = [i, j, k]';
+    R2rot   = [i_feat, j, k]';
 
 %   Rotation from Phobos radial reference frame to Phobos-fixed reference frame
 
@@ -58,20 +55,98 @@ function Y = visible_features(data, file_features, pars, units)
         end
     end
 
-%%   Between those features, which ones are in the FOV?
+    
 
-    M_int = pars.camera_intrinsic;
+%%   Between those features, which ones are inside the FOV?
 
-%   Rotation matrix to the camera fixed frame
+%   Position of MMX wrt Phobos-fixed frame
+    
     r_sb    = MMX(1:3) - Phobos(1:3);
     R_sb    = norm(r_sb);
-    
-    i       = r_sb/R_sb;
+    r_sb_Ph = RLibration*R2rot*r_sb;
+
+%   Rotation matrix from Phobos to the camera les
+    i_feat       = r_sb_Ph/R_sb;
     v       = [0; 0; 1];
-    j       = cross(v, i)/norm(cross(v, i));
-    k       = cross(i, j)/norm(cross(i, j));
+    j       = cross(v, i_feat)/norm(cross(v, i_feat));
+    k       = cross(i_feat, j)/norm(cross(i_feat, j));
+    
+    T       = [i_feat, j, k]';
+
+%   In the CV world si usa questa convenzione, l'asse Z della camera è
+%   quello che esce dal piatto
+    R_bCam2CV   = [0, 0, 1; 0, 1, 0; -1, 0, 0];
+    R_CV        = R_bCam2CV*T;
+
+%     [M_int, M_ext, PM] = ProjMatrix(r_sb_Ph, pars, units);
+
+
+
+    visible = [];
+    Y_LOS   = [];
+    Y_pix   = [];
     
-    T       = [i, j, k]';
+%   MMX-Phobos direction in the Phobos-fixed reference frame
+    I   = i_feat;
+
+    for n = 1:size(point_in_light,2)
+        candidate = point_in_light(:,n);
+
+        r_sf = r_sb_Ph - candidate;
+        
+        i_feat = -r_sf/norm(r_sf);
+        v      = [0; 0; 1];
+        z_cam  = cross(v, I)/norm(cross(v, I));
+        y_cam  = cross(z_cam, I)/norm(cross(z_cam, I));
+
+        angle   = acos(dot(I,-i_feat));
+
+        if (angle<pars.FOV)&&(dot(r_sb_Ph, point_in_light(:,n))>0)
+            visible     = [candidate, visible];
+            LOS_feat    = [y_cam'; z_cam'] * r_sf/norm(r_sf) + [random('Normal',0, pars.ObsNoise.camera);...
+                               random('Normal',0, pars.ObsNoise.camera)];
+            Y_LOS = [LOS_feat, Y_LOS];
+
+            [~, ~, PM]  = ProjMatrix(-r_sb_Ph, pars, units);
+            pix_feat    = PM*[-candidate; 1];
+            pix_feat    = pix_feat./repmat(pix_feat(3),3,1);
+            Y_pix = [[ceil(pix_feat(1:2)+ [random('Normal',0, 1); random('Normal',0, 1)]); candidate(end)], Y_pix];
+
+        end
+    end
+
+    figure()
+    grid on
+    axis equal
+    hold on
+    plot3(points(1,:), points(2,:), points(3,:),'.','Color','b')
+    plot3(point_in_light(1,:), point_in_light(2,:), point_in_light(3,:), 'x','Color','g')
+    if ~isempty(visible)
+        plot3(visible(1,:), visible(2,:), visible(3,:), 'o','Color','r')
+        for n = 1:size(visible,2)    
+            r_sf    = r_sb_Ph - visible(:,n);
+            LOS     = -r_sf/norm(r_sf);
+            quiver3(r_sb_Ph(1),r_sb_Ph(2),r_sb_Ph(3),LOS(1)*units.lsf,LOS(2)*units.lsf,LOS(3)*units.lsf);
+        end
+    end
+    xlabel('X','FontSize',16,'Interpreter','latex')
+    ylabel('Y','FontSize',16,'Interpreter','latex')
+    zlabel('Z','FontSize',16,'Interpreter','latex')
+    quiver3(-2*r_PhSun(1)*units.lsf,-2*r_PhSun(2)*units.lsf,-2*r_PhSun(3)*units.lsf,...
+        r_PhSun(1)*units.lsf,r_PhSun(2)*units.lsf,r_PhSun(3)*units.lsf,'LineWidth',1.5,'Color','g')
+    plot3(-2*r_PhSun(1)*units.lsf,-2*r_PhSun(2)*units.lsf,-2*r_PhSun(3)*units.lsf, '*','Color','g','MarkerSize',20)
+    quiver3(r_sb_Ph(1),r_sb_Ph(2),r_sb_Ph(3),...
+        -r_sb_Ph(1)/units.tsf,-r_sb_Ph(2)/units.tsf,-r_sb_Ph(3)/units.tsf,'LineWidth',1.5,'Color','r')
+    plotCamera('location',r_sb_Ph,'orientation',R_CV,'Color','r')
 
+    figure()
+    grid on
+    axis equal
+    hold on
+    plot(Y_pix(1,:), Y_pix(2,:), 'o', 'Color', 'r');
+    xlabel('X','FontSize',16,'Interpreter','latex')
+    ylabel('Y','FontSize',16,'Interpreter','latex')
+    xlim([0, 2*pars.camera_intrinsic(3,1)])
+    ylim([0, 2*pars.camera_intrinsic(3,2)])
 
 end
\ No newline at end of file