diff --git a/change.txt b/change.txt index 07dfd137ca..2bc3dd8261 100644 --- a/change.txt +++ b/change.txt @@ -14,6 +14,8 @@ Version : 1.1.2 * Better handling when the camera becomes inaccessible while it was reading it - WorldToCameraToPixel * Supports homogenous coordinates +- BundleAdjustmentMetricResidualFunction + * Fixed bug with homogeneous coordinates and rigid objects --------------------------------------------- Date : 2023/Sep/24 diff --git a/main/boofcv-geo/src/main/java/boofcv/alg/geo/bundle/BundleAdjustmentMetricResidualFunction.java b/main/boofcv-geo/src/main/java/boofcv/alg/geo/bundle/BundleAdjustmentMetricResidualFunction.java index f36ec36df0..2389719d6b 100644 --- a/main/boofcv-geo/src/main/java/boofcv/alg/geo/bundle/BundleAdjustmentMetricResidualFunction.java +++ b/main/boofcv-geo/src/main/java/boofcv/alg/geo/bundle/BundleAdjustmentMetricResidualFunction.java @@ -57,6 +57,7 @@ public class BundleAdjustmentMetricResidualFunction // feature location in world coordinates private final Point3D_F64 worldPt = new Point3D_F64(); + private final Point4D_F64 worldPt4 = new Point4D_F64(); // number of parameters being optimised private int numParameters; @@ -252,8 +253,8 @@ private void project4( double[] output ) { objectPt.get(p4); // Transform to world frame and from world to camera - SePointOps_F64.transformV(rigid.object_to_world, p4, worldPt); - SePointOps_F64.transform(world_to_view, worldPt, cameraPt); + SePointOps_F64.transform(rigid.object_to_world, p4, worldPt4); + SePointOps_F64.transformV(world_to_view, worldPt4, cameraPt); camera.model.project(cameraPt.x, cameraPt.y, cameraPt.z, predictedPixel); diff --git a/main/boofcv-geo/src/test/java/boofcv/alg/geo/bundle/TestBundleAdjustmentMetricResidualFunction.java b/main/boofcv-geo/src/test/java/boofcv/alg/geo/bundle/TestBundleAdjustmentMetricResidualFunction.java index f95e0a14c7..6c17a06334 100644 --- a/main/boofcv-geo/src/test/java/boofcv/alg/geo/bundle/TestBundleAdjustmentMetricResidualFunction.java +++ b/main/boofcv-geo/src/test/java/boofcv/alg/geo/bundle/TestBundleAdjustmentMetricResidualFunction.java @@ -21,8 +21,13 @@ import boofcv.abst.geo.bundle.SceneObservations; import boofcv.abst.geo.bundle.SceneStructureCommon; import boofcv.abst.geo.bundle.SceneStructureMetric; +import boofcv.alg.geo.WorldToCameraToPixel; import boofcv.alg.geo.bundle.cameras.BundleZoomState; +import boofcv.struct.calib.CameraPinholeBrown; import boofcv.testing.BoofStandardJUnit; +import georegression.struct.point.Point2D_F64; +import georegression.struct.point.Point4D_F64; +import georegression.struct.se.SpecialEuclideanOps_F64; import org.ejml.UtilEjml; import org.junit.jupiter.api.Test; @@ -217,4 +222,54 @@ static SceneObservations createObservations( Random rand, SceneStructureMetric s return obs; } + + /** + * Test to see if rigid objects are projected correctly. This is related to a bug that was found. + */ + @Test void rigidObjectProjectionH() { + // Create a simple scene with a simple rigid object + var worldToObject = SpecialEuclideanOps_F64.eulerXyz(0.1, -0.05, 0.04, 0.04, -0.03, 0.09, null); + var worldToView = SpecialEuclideanOps_F64.eulerXyz(0.15, -0.08, 0.04, 0.09, -0.01, 0.09, null); + var brown = new CameraPinholeBrown().fsetK(500, 500, 0, 250, 250, 1000, 1000).fsetRadial(0.005, -0.004); + + var structure = new SceneStructureMetric(true); + structure.initialize(1, 1, 1, 0, 1); + structure.setRigid(0, true, worldToObject, 1); + structure.setView(0, 0, true, worldToView); + structure.setCamera(0, true, brown); + structure.assignIDsToRigidPoints(); + structure.getRigid(0).setPoint(0, 0.2, 0.3, -1.1, -0.96); + + structure.rigids.get(0).connectPointToView(0, 0); + + SceneObservations obs = createObservations(rand, structure); + + var param = new double[structure.getParameterCount()]; + + new CodecSceneStructureMetric().encode(structure, param); + + var alg = new BundleAdjustmentMetricResidualFunction(); + alg.configure(structure, obs); + + var found = new double[alg.getNumOfOutputsM()]; + alg.process(param, found); + + var rigid4 = new Point4D_F64(); + var world4 = new Point4D_F64(); + var predicted = new Point2D_F64(); + var observed = new Point2D_F64(); + var w2p = new WorldToCameraToPixel(); + w2p.configure(brown, worldToView); + for (int i = 0; i < structure.rigids.size; i++) { + // For the one point on the rigid object, compute where it should be + SceneStructureMetric.Rigid rigid = structure.rigids.get(i); + rigid.getPoint(0, rigid4); + worldToObject.transform(rigid4, world4); + assertTrue(w2p.transform(world4, predicted)); + + obs.viewsRigid.get(0).getPixel(0, observed); + assertEquals(predicted.x - observed.x, found[0], UtilEjml.TEST_F64); + assertEquals(predicted.y - observed.y, found[1], UtilEjml.TEST_F64); + } + } } diff --git a/main/boofcv-geo/src/test/java/boofcv/alg/geo/bundle/TestCodecSceneStructureMetric.java b/main/boofcv-geo/src/test/java/boofcv/alg/geo/bundle/TestCodecSceneStructureMetric.java index ce5a765fd8..21a2169ef6 100644 --- a/main/boofcv-geo/src/test/java/boofcv/alg/geo/bundle/TestCodecSceneStructureMetric.java +++ b/main/boofcv-geo/src/test/java/boofcv/alg/geo/bundle/TestCodecSceneStructureMetric.java @@ -123,8 +123,10 @@ static SceneStructureMetric createScene( Random rand, boolean homogenous, boolea SceneStructureMetric.Rigid r = out.rigids.data[i]; if (homogenous) { for (int j = 0; j < r.points.length; j++) { + // sign shouldn't matter + double sign = rand.nextBoolean() ? -1 : 1; double w = rand.nextDouble()*3 + 0.5; - r.setPoint(j, rand.nextGaussian()*0.2, rand.nextGaussian()*0.1, rand.nextGaussian()*0.2, w); + r.setPoint(j, sign*rand.nextGaussian()*0.2, sign*rand.nextGaussian()*0.1, sign*rand.nextGaussian()*0.2, sign*w); } } else { for (int j = 0; j < r.points.length; j++) { @@ -150,7 +152,9 @@ static SceneStructureMetric createScene( Random rand, boolean homogenous, boolea if (homogenous) { for (int i = 0; i < out.points.size; i++) { - double w = rand.nextDouble()*0.5 + 0.5; + // sign shouldn't matter + double sign = rand.nextBoolean() ? -1 : 1; + double w = sign*(rand.nextDouble()*0.5 + 0.5); out.setPoint(i, w*(i + 1), w*(i + 2*rand.nextGaussian()), w*(2*i - 3*rand.nextGaussian()), w); } } else {