- WorldToCameraToPixel

  * Supports homogenous coordinates

change.txt

@@ -12,6 +12,8 @@ Version : 1.1.2
 
 - Android
   * Better handling when the camera becomes inaccessible while it was reading it
+- WorldToCameraToPixel
+  * Supports homogenous coordinates
 
 ---------------------------------------------
 Date    : 2023/Sep/24

main/boofcv-geo/src/main/java/boofcv/alg/geo/WorldToCameraToPixel.java

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Peter Abeles. All Rights Reserved.
+ * Copyright (c) 2023, Peter Abeles. All Rights Reserved.
  *
  * This file is part of BoofCV (http://boofcv.org).
  *
@@ -26,6 +26,7 @@
 import boofcv.struct.distort.Point2Transform2_F64;
 import georegression.struct.point.Point2D_F64;
 import georegression.struct.point.Point3D_F64;
+import georegression.struct.point.Point4D_F64;
 import georegression.struct.se.Se3_F64;
 import georegression.transform.se.SePointOps_F64;
 import lombok.Getter;
@@ -46,6 +47,8 @@ public class WorldToCameraToPixel {
 	/** storage for point in camera frame */
 	@Getter private Point3D_F64 cameraPt = new Point3D_F64();
 
+	@Getter private Point4D_F64 cameraPt4 = new Point4D_F64();
+
 	/** transform from normalized image coordinates into pixels */
 	@Getter private Point2Transform2_F64 normToPixel;
 
@@ -108,6 +111,25 @@ public boolean transform( Point3D_F64 worldPt, Point2D_F64 pixelPt, Point2D_F64
 		return true;
 	}
 
+	/**
+	 * Computes the observed location of the specified point in world coordinates in the camera pixel. If
+	 * the object can't be viewed because it is behind the camera then false is returned.
+	 *
+	 * @param worldPt Location of point in world frame. Homogenous coordinates.
+	 * @param pixelPt Pixel observation of point.
+	 * @return True if visible (+z) or false if not visible (-z)
+	 */
+	public boolean transform( Point4D_F64 worldPt, Point2D_F64 pixelPt ) {
+		SePointOps_F64.transform(worldToCamera, worldPt, cameraPt4);
+
+		// can't see the point
+		if (PerspectiveOps.isBehindCamera(cameraPt4))
+			return false;
+
+		normToPixel.compute(cameraPt4.x/cameraPt4.z, cameraPt4.y/cameraPt4.z, pixelPt);
+		return true;
+	}
+
 	/**
 	 * Computes location of 3D point in world as observed in the camera. Point is returned if visible or null
 	 * if not visible.

main/boofcv-geo/src/test/java/boofcv/alg/geo/TestWorldToCameraToPixel.java

@@ -24,52 +24,65 @@
 import boofcv.testing.BoofStandardJUnit;
 import georegression.struct.point.Point2D_F64;
 import georegression.struct.point.Point3D_F64;
+import georegression.struct.point.Point4D_F64;
 import georegression.struct.se.Se3_F64;
 import georegression.transform.se.SePointOps_F64;
 import org.junit.jupiter.api.Test;
 
-import static org.junit.jupiter.api.Assertions.assertFalse;
-import static org.junit.jupiter.api.Assertions.assertTrue;
+import static org.junit.jupiter.api.Assertions.*;
 
 public class TestWorldToCameraToPixel extends BoofStandardJUnit {
-	CameraPinholeBrown intrinsic = new CameraPinholeBrown(500,500,0,320,240,640,480).fsetRadial(-0.1,-0.05);
-	Point2Transform2_F64 normToPixel = LensDistortionFactory.narrow(intrinsic).distort_F64(false,true);
+	CameraPinholeBrown intrinsic = new CameraPinholeBrown(500, 500, 0, 320, 240, 640, 480).fsetRadial(-0.1, -0.05);
+	Point2Transform2_F64 normToPixel = LensDistortionFactory.narrow(intrinsic).distort_F64(false, true);
 
 	Se3_F64 worldToCamera = new Se3_F64();
 
-	Point3D_F64 infront = new Point3D_F64(-0.1,0.2,0);
-	Point3D_F64 behind = new Point3D_F64(-0.1,0.2,-4);
+	Point3D_F64 infront = new Point3D_F64(-0.1, 0.2, 0);
+	Point3D_F64 behind = new Point3D_F64(-0.1, 0.2, -4);
+
+	Point4D_F64 infront4 = new Point4D_F64(-0.2, 0.4, 0, 2.0);
+	Point4D_F64 behind4 = new Point4D_F64(0.2, -0.4, 8, -2.0);
 
 	Point2D_F64 expectedInFront = new Point2D_F64();
 
 	public TestWorldToCameraToPixel() {
-		worldToCamera.getT().setTo(0,0,3);
+		worldToCamera.getT().setTo(0, 0, 3);
 
-		Point3D_F64 tmp = new Point3D_F64();
-		SePointOps_F64.transform(worldToCamera,infront,tmp);
-		normToPixel.compute(tmp.x/tmp.z,tmp.y/tmp.z,expectedInFront);
+		var tmp = new Point3D_F64();
+		SePointOps_F64.transform(worldToCamera, infront, tmp);
+		normToPixel.compute(tmp.x/tmp.z, tmp.y/tmp.z, expectedInFront);
 	}
 
 	@Test void transform_two() {
+		var worldToPixel = new WorldToCameraToPixel();
+		worldToPixel.configure(intrinsic, worldToCamera);
 
-		WorldToCameraToPixel worldToPixel = new WorldToCameraToPixel();
-		worldToPixel.configure(intrinsic,worldToCamera);
-
-		Point2D_F64 found = new Point2D_F64();
+		var found = new Point2D_F64();
 
-		assertTrue(worldToPixel.transform(infront,found));
-		assertTrue(found.distance(expectedInFront)<1e-8);
+		assertTrue(worldToPixel.transform(infront, found));
+		assertTrue(found.distance(expectedInFront) < 1e-8);
 		assertFalse(worldToPixel.transform(behind, found));
 	}
 
 	@Test void transform_one() {
-		WorldToCameraToPixel worldToPixel = new WorldToCameraToPixel();
-		worldToPixel.configure(intrinsic,worldToCamera);
+		var worldToPixel = new WorldToCameraToPixel();
+		worldToPixel.configure(intrinsic, worldToCamera);
 
-		Point2D_F64 found = worldToPixel.transform(infront);
+		var found = worldToPixel.transform(infront);
 
 		assertTrue(found != null);
-		assertTrue(found.distance(expectedInFront)<1e-8);
+		assertTrue(found.distance(expectedInFront) < 1e-8);
 		assertTrue(null == worldToPixel.transform(behind));
 	}
+
+	@Test void transform4_two() {
+		var worldToPixel = new WorldToCameraToPixel();
+		worldToPixel.configure(intrinsic, worldToCamera);
+
+		var found = new Point2D_F64();
+
+		assertTrue(worldToPixel.transform(infront4, found));
+		assertEquals(0.0, found.distance(expectedInFront), 1e-8);
+		assertFalse(worldToPixel.transform(behind4, found));
+	}
 }