diff --git a/change.txt b/change.txt index 135800b2c6..07dfd137ca 100644 --- a/change.txt +++ b/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 diff --git a/main/boofcv-geo/src/main/java/boofcv/alg/geo/WorldToCameraToPixel.java b/main/boofcv-geo/src/main/java/boofcv/alg/geo/WorldToCameraToPixel.java index 332d302b27..19c7459a75 100644 --- a/main/boofcv-geo/src/main/java/boofcv/alg/geo/WorldToCameraToPixel.java +++ b/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. diff --git a/main/boofcv-geo/src/test/java/boofcv/alg/geo/TestWorldToCameraToPixel.java b/main/boofcv-geo/src/test/java/boofcv/alg/geo/TestWorldToCameraToPixel.java index d5caede3aa..0fe1076091 100644 --- a/main/boofcv-geo/src/test/java/boofcv/alg/geo/TestWorldToCameraToPixel.java +++ b/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)); + } }