- BundleAdjustmentMetricResidualFunction

  * Fixed bug with homogeneous coordinates and rigid objects

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

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);
 

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);
+		}
+	}
 }

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 {