Added target annotations

src/main/java/com/elphel/imagej/cuas/CuasMotion.java

@@ -292,6 +292,7 @@ public class CuasMotion {
 		boolean no_border=       clt_parameters.imp.cuas_no_border; // true;
 	// Moving target LMA	
 	    double  lma_sigma =      clt_parameters.imp.cuas_lma_sigma; // =     3.0;
+	    double  wnd_pedestal =   clt_parameters.imp.cuas_wnd_pedestal;    // =  0.1;
 	    double  lma_r0 =         clt_parameters.imp.cuas_lma_r0; // =        3.0;  //maximum with with overshoot
 	    double  lma_ovrsht =     clt_parameters.imp.cuas_lma_ovrsht; // =    2.0;
 	// CUAS Motion LMA parameters
@@ -385,6 +386,8 @@ public class CuasMotion {
 			gd.addMessage("=== Moving target LMA ===");
 			gd.addNumericField("Weight Gaussian sigma",                  lma_sigma, 5,8,"pix",
 					"Weights to emphasize maximum center area when fitting.");
+			gd.addNumericField("Weight pedestal",                        wnd_pedestal, 5,8,"",
+					"Add constant to Gaussian weights.");
 			gd.addNumericField("Target typical radius",                  lma_r0, 5,8,"pix",
 					"Typical target radius including negative overshoot (caused by UM filter).");
 			gd.addNumericField("Target maximum overshoot",               lma_ovrsht, 5,8,"",
@@ -501,6 +504,7 @@ public class CuasMotion {
 			target_frac =   IntersceneMatchParameters.stringToDouble2d(gd.getNextString());
 			no_border =                 gd.getNextBoolean();   
 			lma_sigma =                 gd.getNextNumber();
+			wnd_pedestal =              gd.getNextNumber();
 			lma_r0 =                    gd.getNextNumber();
 			lma_ovrsht =                gd.getNextNumber();
 			lma_fit_xy =                gd.getNextBoolean();
@@ -578,6 +582,7 @@ public class CuasMotion {
 				}
 				clt_parameters.imp.cuas_no_border =   no_border; 
 				clt_parameters.imp.cuas_lma_sigma =   lma_sigma;
+				clt_parameters.imp.cuas_wnd_pedestal= wnd_pedestal;
 				clt_parameters.imp.cuas_lma_r0 =      lma_r0;
 				clt_parameters.imp.cuas_lma_fit_xy=   lma_fit_xy;
 				clt_parameters.imp.cuas_lma_fit_a=    lma_fit_a; 
@@ -652,6 +657,7 @@ public class CuasMotion {
 				System.out.println("target_frac=     "+IntersceneMatchParameters.double2dToString(target_frac));
 				System.out.println("no_border=       "+no_border);
 				System.out.println("lma_sigma=       "+lma_sigma);
+				System.out.println("wnd_pedestal=    "+wnd_pedestal);
 				System.out.println("lma_r0=          "+lma_r0);
 				System.out.println("lma_ovrsht=      "+lma_ovrsht);
 				System.out.println("lma_fit_xy=      "+lma_fit_xy);
@@ -1011,6 +1017,7 @@ public class CuasMotion {
 			final boolean       no_border,
 		// Moving target LMA	
 			final double        lma_sigma,
+			final double        wnd_pedestal,
 			final double        lma_r0,
 			final double        lma_ovrsht,
 		// CUAS Motion LMA parameters
@@ -1053,7 +1060,8 @@ public class CuasMotion {
 					double [] pix_tile = new double [tile2 * tile2];
 					CuasMotionLMA cuasMotionLMA = new CuasMotionLMA(
 							tile2,      // int         width,
-							lma_sigma); // double     sigma); 
+							lma_sigma, // double     sigma);
+							wnd_pedestal);
 					// may be faster if process only where vector_field[nseq][ntile] is not null
 					for (int nSeq = ai.getAndIncrement(); nSeq < num_seq; nSeq = ai.getAndIncrement()) {
 						for (int ntile = 0; ntile < num_tiles; ntile++) {
@@ -2406,9 +2414,22 @@ public class CuasMotion {
 		final Thread[] threads = ImageDtt.newThreadArray(QuadCLT.THREADS_MAX);
 		final AtomicInteger ai = new AtomicInteger(0);
 //		String diamond_path = "/media/elphel/NVME/lwir16-proc/eagle_mountain/graphics/diamond21x17.png";
-		final boolean annotate_mono = clt_parameters.imp.annotate_mono; 
+		final boolean annotate_mono =             clt_parameters.imp.annotate_mono; 
 		final boolean annotate_transparent_mono = clt_parameters.imp.annotate_transparent_mono;
-		final Color annotate_color_mono =  clt_parameters.imp.annotate_color_mono;
+		final Color annotate_color_mono =         clt_parameters.imp.annotate_color_mono;
+		
+		final boolean annotate =   clt_parameters.imp.cuas_annotate;
+		final Color   text_color = clt_parameters.imp.cuas_text_color;
+		final String  font_name =  clt_parameters.imp.cuas_font_name;
+		final int     font_size =  clt_parameters.imp.cuas_font_size;
+		final int     font_type =  clt_parameters.imp.cuas_font_type;
+		final double  ifov =       clt_parameters.imp.cuas_ifov;
+		final int     px0 =        clt_parameters.imp.cuas_px0;
+		final int     py0 =        clt_parameters.imp.cuas_py0;
+		final double  az0 =        clt_parameters.imp.cuas_az0;
+		final double  el0 =        clt_parameters.imp.cuas_el0;
+		final int     space_before_text = 2;
+		
 		ColorProcessor diamond_cp = null;
 		try {
 			diamond_cp = new ColorProcessor(ImageIO.read(new File(diamond_path)));
@@ -2507,7 +2528,9 @@ public class CuasMotion {
 		final ImageStack fstack_scenes = imp.getImageStack();
 		final int posX=  width - scale * 119; // 521;
 		final int posY= height + scale *   1;  // 513;
-		final Font font = new Font("Monospaced", Font.PLAIN, scale2x? 24: 12);
+		final Font font = new Font("Monospaced", Font.PLAIN, scale * 12);
+		final Font font_target = new Font(font_name, font_type, scale * font_size);
+		final boolean target_text_transparent = true;
 		final int nSlices = fstack_scenes.getSize();
 		if (annotate_mono) {
 			ai.set(0);
@@ -2584,6 +2607,21 @@ public class CuasMotion {
 												}
 											}
 										}
+										if (annotate) {
+											int text_left = xl + diamond_width + space_before_text * scale;
+											int text_top =  yt + scale * font_size; // text start from the bottom of the first line
+											ImageProcessor ip = fstack_scenes.getProcessor(nscene+1);
+											String txt = getTargetText(
+													clt_parameters,    // CLTParameters       clt_parameters,
+													targets[ntarget]); // double [] target);
+											ip.setColor(text_color);
+											ip.setFont(font_target);
+											if (target_text_transparent) {
+												ip.drawString(txt, text_left, text_top); // transparent.
+											} else {
+												ip.drawString(txt, text_left, text_top, Color.BLACK); // on black background
+											}
+										}
 									}
 								}
 							}
@@ -2597,6 +2635,54 @@ public class CuasMotion {
 		return imp;
 	}
 	
+	public static String getTargetText(
+			CLTParameters       clt_parameters,
+			double [] target) {
+		double target_x = target[TARGET_X];
+		double target_y = target[TARGET_Y];
+		double target_vx = target[TARGET_VX];
+		double target_vy = target[TARGET_VY];
+		String omega = "\u03A9";
+		double fps = 60.0;
+		String number_format = "%3.0f";
+		String omega_format =  "%3.1f";
+		
+		double   ifov =  clt_parameters.imp.cuas_ifov; // 0.05;  // degree per pixel
+		int      px0 =   clt_parameters.imp.cuas_px0;  // 283;   // pixel with known azimuth 
+		int      py0 =   clt_parameters.imp.cuas_py0;  // 386;   // pixel with known elevation
+		double   az0 =   clt_parameters.imp.cuas_az0;  // 201.5; // degrees for cuas_px0;
+		double   el0 =   clt_parameters.imp.cuas_el0;  // 0.0;   // degrees for cuas_px0;
+		double az =  (target_x - px0)*ifov+az0;
+		double el =  -(target_y - py0)*ifov+el0;
+		double omega_az =  target_vx * ifov * fps;
+		double omega_el = -target_vy * ifov * fps;
+		while (az < 0) {
+			az += 360;
+		}
+		while (az >= 360) {
+			az -= 360;
+		}
+		while (el < -180) {
+			el += 360;
+		}
+		while (el > 180) {
+			el -= 360;
+		}
+		String txt = "";
+		txt += " AZ  "+String.format(number_format,az)+"\n";
+		txt += " EL "+ getSignedDouble(el,number_format)+"\n";
+		txt += omega+"AZ "+getSignedDouble(omega_az,omega_format)+"\n";
+		txt += omega+"EL "+getSignedDouble(omega_el,omega_format);
+		return txt;
+	}
+	public static String getSignedDouble(
+			double d,
+			String format) {
+		String s = (d > 0)? "+" : ( (d <0)? "-":" ");
+		return s+ String.format(format, Math.abs(d));
+	}
+	
+	
 	public static String saveAsVideo(
 			CLTParameters       clt_parameters,
 			QuadCLT             scene,     // if not null - use it's model directory
@@ -2778,6 +2864,7 @@ public class CuasMotion {
 		boolean no_border=       clt_parameters.imp.cuas_no_border; // true;
 	// Moving target LMA	
 	    double  lma_sigma =      clt_parameters.imp.cuas_lma_sigma; // =     3.0;
+	    double  wnd_pedestal =   clt_parameters.imp.cuas_wnd_pedestal; // =     3.0;
 	    double  lma_r0 =         clt_parameters.imp.cuas_lma_r0; // =        3.0;  //maximum with with overshoot
 	    double  lma_ovrsht =     clt_parameters.imp.cuas_lma_ovrsht; // =    2.0;
 	// CUAS Motion LMA parameters
@@ -3075,6 +3162,7 @@ public class CuasMotion {
 					no_border,            // final boolean       no_border,
 					// Moving target LMA	
 				    lma_sigma,            // final double        lma_sigma,
+					wnd_pedestal,         // final double        wnd_pedestal,
 				    lma_r0,               // final double        lma_r0,
 				    lma_ovrsht,  // final double        lma_ovrsht,
 				// CUAS Motion LMA parameters
@@ -3277,7 +3365,9 @@ public class CuasMotion {
 		final double [][] target_frac = new double [clt_parameters.imp.cuas_target_frac.length][2];
 		
 		// Moving target LMA	
-	    double  lma_sigma =      clt_parameters.imp.cuas_lma_sigma; // =     3.0;
+	    double  lma_sigma =      clt_parameters.imp.cuas_lma_sigma;       // =  3.0;
+	    double  wnd_pedestal =   clt_parameters.imp.cuas_wnd_pedestal;    // =  0.1;
+	    
 	    double  lma_r0 =         clt_parameters.imp.cuas_lma_r0; // =        3.0;  //maximum with with overshoot
 	    double  lma_ovrsht =     clt_parameters.imp.cuas_lma_ovrsht; // =    2.0;
 	// CUAS Motion LMA parameters
@@ -3361,6 +3451,7 @@ public class CuasMotion {
 				no_border,            // final boolean       no_border,
 				// Moving target LMA	
 			    lma_sigma,            // final double        lma_sigma,
+				wnd_pedestal,         // final double        wnd_pedestal,
 			    lma_r0,               // final double        lma_r0,
 			    lma_ovrsht,  // final double        lma_ovrsht,
 			// CUAS Motion LMA parameters

src/main/java/com/elphel/imagej/cuas/CuasMotionLMA.java

@@ -64,13 +64,14 @@ public class CuasMotionLMA {
 
 	public CuasMotionLMA(
 			int         width,
-			double     sigma) {
+			double     sigma,
+			double     wnd_pedestal) {
 		this.width = width;
 		window = new double [width][width];
 		double k = -0.5/(sigma*sigma);
 		for (int i = 0; i < width; i++) {
 			for (int j = 0; j < width; j++) {
-				window[i][j] = Math.exp(k*(i*i+j*j));
+				window[i][j] = Math.exp(k*(i*i+j*j)) + wnd_pedestal; // will be normalized when used
 			}
 		}
 	}