BundleAdjustmentMetricSchurJacobian

- Fixed incorrect jacobian
lessthanoptimal · Nov 5, 2023 · 310fdc3 · 310fdc3
1 parent a0fcbbd
commit 310fdc3
Showing 1 changed file with 8 additions and 3 deletions.
diff --git a/main/boofcv-geo/src/main/java/boofcv/alg/geo/bundle/BundleAdjustmentMetricSchurJacobian.java b/main/boofcv-geo/src/main/java/boofcv/alg/geo/bundle/BundleAdjustmentMetricSchurJacobian.java
@@ -389,12 +389,13 @@ private int computeRigidPoints( DMatrix leftPoint, DMatrix rightView,
 
 			if (structure.isHomogenous()) {
 				rigid.getPoint(pointIndex, rigidPt4);
-				SePointOps_F64.transformV(rigid.object_to_world, rigidPt4, worldPt3);
+				SePointOps_F64.transform(rigid.object_to_world, rigidPt4, worldPt4);
+				SePointOps_F64.transformV(world_to_view, worldPt4, cameraPt);
 			} else {
 				rigid.getPoint(pointIndex, rigidPt3);
 				SePointOps_F64.transform(rigid.object_to_world, rigidPt3, worldPt3);
+				SePointOps_F64.transform(world_to_view, worldPt3, cameraPt);
 			}
-			SePointOps_F64.transform(world_to_view, worldPt3, cameraPt);
 
 			jacRowX = observationIndex*2;
 			jacRowY = jacRowX + 1;
@@ -416,7 +417,11 @@ private int computeRigidPoints( DMatrix leftPoint, DMatrix rightView,
 			}
 
 			//============ Partial of world to view
-			partialViewSE3(rightView, view, worldPt3.x, worldPt3.y, worldPt3.z, 1);
+			if (structure.isHomogenous()) {
+				partialViewSE3(rightView, view, worldPt4.x, worldPt4.y, worldPt4.z, worldPt4.w);
+			} else {
+				partialViewSE3(rightView, view, worldPt3.x, worldPt3.y, worldPt3.z, 1);
+			}
 
 			//============ Partial of body to world
 			// R2*(R1*X+T1)+T2