Add Weighting to the Kabsch Solver

Victorique-GOSICK · Oct 7, 2017 · 98d1e4d · 98d1e4d
1 parent e0d26e3
commit 98d1e4d
Show file tree

Hide file tree

Showing 18 changed files with 247 additions and 100 deletions.
diff --git a/Assets/BundleAdustment.meta b/Assets/BundleAdustment.meta
diff --git a/Assets/BundleAdustment/BundleAdjuster.cs b/Assets/BundleAdustment/BundleAdjuster.cs
@@ -7,9 +7,9 @@ public class BundleAdjuster : MonoBehaviour {
   Vector3[][] rayDirections;
 
   KabschSolver solver = new KabschSolver();
-  Vector3[] inPoints, refPoints;
+  Vector4[] inPoints; Vector3[] refPoints;
   void Start () {
-    inPoints = new Vector3[features.Length];
+    inPoints = new Vector4[features.Length];
     refPoints = new Vector3[features.Length];
     rayDirections = new Vector3[2][];
 
@@ -33,8 +33,10 @@ void Update() {
         //Take the abs of the times so they can't intersect behind the camera
         timeLineOne = Mathf.Abs(timeLineOne); timeLineTwo = Mathf.Abs(timeLineTwo);
 
-        inPoints[i] = Vector3.LerpUnclamped(cameras[0].position, cameras[0].TransformPoint(rayDirections[0][i]), timeLineOne);
+        Vector3 tempInPoint = Vector3.LerpUnclamped(cameras[0].position, cameras[0].TransformPoint(rayDirections[0][i]), timeLineOne);
         refPoints[i] = Vector3.LerpUnclamped(cameras[1].position, cameras[1].TransformPoint(rayDirections[1][i]), timeLineTwo);
+        inPoints[i] = new Vector4(tempInPoint.x, tempInPoint.y, tempInPoint.z, 1f);//(tempInPoint-refPoints[i]).sqrMagnitude);
+
         Debug.DrawLine(inPoints[i], refPoints[i], Color.red);
       }
 

diff --git a/Assets/BundleAdustment/MonocularTo3DAdjustment.unity b/Assets/BundleAdustment/MonocularTo3DAdjustment.unity
@@ -77,15 +77,17 @@ LightmapSettings:
     m_PVRDirectSampleCount: 32
     m_PVRSampleCount: 500
     m_PVRBounces: 2
-    m_PVRFiltering: 0
+    m_PVRFilterTypeDirect: 0
+    m_PVRFilterTypeIndirect: 0
+    m_PVRFilterTypeAO: 0
     m_PVRFilteringMode: 1
     m_PVRCulling: 1
     m_PVRFilteringGaussRadiusDirect: 1
     m_PVRFilteringGaussRadiusIndirect: 5
     m_PVRFilteringGaussRadiusAO: 2
-    m_PVRFilteringAtrousColorSigma: 1
-    m_PVRFilteringAtrousNormalSigma: 1
-    m_PVRFilteringAtrousPositionSigma: 1
+    m_PVRFilteringAtrousPositionSigmaDirect: 0.5
+    m_PVRFilteringAtrousPositionSigmaIndirect: 2
+    m_PVRFilteringAtrousPositionSigmaAO: 1
   m_LightingDataAsset: {fileID: 0}
   m_UseShadowmask: 0
 --- !u!196 &4
@@ -107,6 +109,8 @@ NavMeshSettings:
     manualTileSize: 0
     tileSize: 256
     accuratePlacement: 0
+    debug:
+      m_Flags: 0
   m_NavMeshData: {fileID: 0}
 --- !u!1 &153165801
 GameObject:
@@ -134,6 +138,7 @@ MeshRenderer:
   m_Enabled: 1
   m_CastShadows: 1
   m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
   m_MotionVectors: 1
   m_LightProbeUsage: 1
   m_ReflectionProbeUsage: 1
@@ -149,6 +154,7 @@ MeshRenderer:
   m_PreserveUVs: 1
   m_IgnoreNormalsForChartDetection: 0
   m_ImportantGI: 0
+  m_StitchLightmapSeams: 0
   m_SelectedEditorRenderState: 3
   m_MinimumChartSize: 4
   m_AutoUVMaxDistance: 0.5
@@ -203,6 +209,7 @@ MeshRenderer:
   m_Enabled: 1
   m_CastShadows: 1
   m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
   m_MotionVectors: 1
   m_LightProbeUsage: 1
   m_ReflectionProbeUsage: 1
@@ -218,6 +225,7 @@ MeshRenderer:
   m_PreserveUVs: 1
   m_IgnoreNormalsForChartDetection: 0
   m_ImportantGI: 0
+  m_StitchLightmapSeams: 0
   m_SelectedEditorRenderState: 3
   m_MinimumChartSize: 4
   m_AutoUVMaxDistance: 0.5
@@ -272,6 +280,7 @@ MeshRenderer:
   m_Enabled: 1
   m_CastShadows: 1
   m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
   m_MotionVectors: 1
   m_LightProbeUsage: 1
   m_ReflectionProbeUsage: 1
@@ -287,6 +296,7 @@ MeshRenderer:
   m_PreserveUVs: 1
   m_IgnoreNormalsForChartDetection: 0
   m_ImportantGI: 0
+  m_StitchLightmapSeams: 0
   m_SelectedEditorRenderState: 3
   m_MinimumChartSize: 4
   m_AutoUVMaxDistance: 0.5
@@ -341,6 +351,7 @@ MeshRenderer:
   m_Enabled: 1
   m_CastShadows: 1
   m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
   m_MotionVectors: 1
   m_LightProbeUsage: 1
   m_ReflectionProbeUsage: 1
@@ -356,6 +367,7 @@ MeshRenderer:
   m_PreserveUVs: 1
   m_IgnoreNormalsForChartDetection: 0
   m_ImportantGI: 0
+  m_StitchLightmapSeams: 0
   m_SelectedEditorRenderState: 3
   m_MinimumChartSize: 4
   m_AutoUVMaxDistance: 0.5
@@ -441,20 +453,6 @@ Light:
   m_Lightmapping: 4
   m_AreaSize: {x: 1, y: 1}
   m_BounceIntensity: 1
-  m_FalloffTable:
-    m_Table[0]: 0
-    m_Table[1]: 0
-    m_Table[2]: 0
-    m_Table[3]: 0
-    m_Table[4]: 0
-    m_Table[5]: 0
-    m_Table[6]: 0
-    m_Table[7]: 0
-    m_Table[8]: 0
-    m_Table[9]: 0
-    m_Table[10]: 0
-    m_Table[11]: 0
-    m_Table[12]: 0
   m_ColorTemperature: 6570
   m_UseColorTemperature: 0
   m_ShadowRadius: 0
@@ -521,6 +519,7 @@ MeshRenderer:
   m_Enabled: 1
   m_CastShadows: 1
   m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
   m_MotionVectors: 1
   m_LightProbeUsage: 1
   m_ReflectionProbeUsage: 1
@@ -536,6 +535,7 @@ MeshRenderer:
   m_PreserveUVs: 0
   m_IgnoreNormalsForChartDetection: 0
   m_ImportantGI: 0
+  m_StitchLightmapSeams: 0
   m_SelectedEditorRenderState: 3
   m_MinimumChartSize: 4
   m_AutoUVMaxDistance: 0.5
@@ -583,6 +583,7 @@ MeshRenderer:
   m_Enabled: 1
   m_CastShadows: 1
   m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
   m_MotionVectors: 1
   m_LightProbeUsage: 1
   m_ReflectionProbeUsage: 1
@@ -598,6 +599,7 @@ MeshRenderer:
   m_PreserveUVs: 1
   m_IgnoreNormalsForChartDetection: 0
   m_ImportantGI: 0
+  m_StitchLightmapSeams: 0
   m_SelectedEditorRenderState: 3
   m_MinimumChartSize: 4
   m_AutoUVMaxDistance: 0.5
@@ -652,6 +654,7 @@ MeshRenderer:
   m_Enabled: 1
   m_CastShadows: 1
   m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
   m_MotionVectors: 1
   m_LightProbeUsage: 1
   m_ReflectionProbeUsage: 1
@@ -667,6 +670,7 @@ MeshRenderer:
   m_PreserveUVs: 1
   m_IgnoreNormalsForChartDetection: 0
   m_ImportantGI: 0
+  m_StitchLightmapSeams: 0
   m_SelectedEditorRenderState: 3
   m_MinimumChartSize: 4
   m_AutoUVMaxDistance: 0.5
@@ -755,11 +759,11 @@ Camera:
   m_TargetEye: 3
   m_HDR: 0
   m_AllowMSAA: 1
+  m_AllowDynamicResolution: 0
   m_ForceIntoRT: 0
   m_OcclusionCulling: 1
   m_StereoConvergence: 10
   m_StereoSeparation: 0.022
-  m_StereoMirrorMode: 0
 --- !u!1 &1364872344
 GameObject:
   m_ObjectHideFlags: 0
@@ -786,6 +790,7 @@ MeshRenderer:
   m_Enabled: 1
   m_CastShadows: 1
   m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
   m_MotionVectors: 1
   m_LightProbeUsage: 1
   m_ReflectionProbeUsage: 1
@@ -801,6 +806,7 @@ MeshRenderer:
   m_PreserveUVs: 1
   m_IgnoreNormalsForChartDetection: 0
   m_ImportantGI: 0
+  m_StitchLightmapSeams: 0
   m_SelectedEditorRenderState: 3
   m_MinimumChartSize: 4
   m_AutoUVMaxDistance: 0.5
@@ -855,6 +861,7 @@ MeshRenderer:
   m_Enabled: 1
   m_CastShadows: 1
   m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
   m_MotionVectors: 1
   m_LightProbeUsage: 1
   m_ReflectionProbeUsage: 1
@@ -870,6 +877,7 @@ MeshRenderer:
   m_PreserveUVs: 1
   m_IgnoreNormalsForChartDetection: 0
   m_ImportantGI: 0
+  m_StitchLightmapSeams: 0
   m_SelectedEditorRenderState: 3
   m_MinimumChartSize: 4
   m_AutoUVMaxDistance: 0.5
@@ -937,6 +945,7 @@ MonoBehaviour:
   - {fileID: 634320946}
   - {fileID: 1008528448}
   aligningCamera: {fileID: 1275341411}
+  useLinearEstimate: 1
 --- !u!4 &1486378348
 Transform:
   m_ObjectHideFlags: 0
@@ -977,6 +986,7 @@ MeshRenderer:
   m_Enabled: 1
   m_CastShadows: 1
   m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
   m_MotionVectors: 1
   m_LightProbeUsage: 1
   m_ReflectionProbeUsage: 1
@@ -992,6 +1002,7 @@ MeshRenderer:
   m_PreserveUVs: 1
   m_IgnoreNormalsForChartDetection: 0
   m_ImportantGI: 0
+  m_StitchLightmapSeams: 0
   m_SelectedEditorRenderState: 3
   m_MinimumChartSize: 4
   m_AutoUVMaxDistance: 0.5
@@ -1046,6 +1057,7 @@ MeshRenderer:
   m_Enabled: 1
   m_CastShadows: 1
   m_ReceiveShadows: 1
+  m_DynamicOccludee: 1
   m_MotionVectors: 1
   m_LightProbeUsage: 1
   m_ReflectionProbeUsage: 1
@@ -1061,6 +1073,7 @@ MeshRenderer:
   m_PreserveUVs: 1
   m_IgnoreNormalsForChartDetection: 0
   m_ImportantGI: 0
+  m_StitchLightmapSeams: 0
   m_SelectedEditorRenderState: 3
   m_MinimumChartSize: 4
   m_AutoUVMaxDistance: 0.5

diff --git a/Assets/BundleAdustment/SpatialBundleAdjuster.cs b/Assets/BundleAdustment/SpatialBundleAdjuster.cs
@@ -3,16 +3,17 @@
 public class SpatialBundleAdjuster : MonoBehaviour {
   public Transform[] features;
   public Transform aligningCamera;
+  [Tooltip("This makes the convergence much faster if there are no outliers.\n\nTurning this off switches to a slower, more outlier robust formulation.")]
   public bool useLinearEstimate = true;
   Vector3[] rayDirections;
 
   KabschSolver solver = new KabschSolver();
-  Vector3[] inPoints, refPoints;
+  Vector3[] inPoints; Vector4[] refPoints;
   float previousCost = 1000f;
   void Start() {
     rayDirections = new Vector3[features.Length];
     inPoints = new Vector3[features.Length];
-    refPoints = new Vector3[features.Length];
+    refPoints = new Vector4[features.Length];
 
     for (int i = 0; i < features.Length; i++) {
       refPoints[i] = features[i].position;
@@ -27,9 +28,13 @@ void Update() {
       float currentCost = 0f;
 
       for (int i = 0; i < features.Length; i++) {
-        refPoints[i] = features[i].position;
         inPoints[i] = Constraints.ConstrainToSegment(features[i].position, aligningCamera.position, aligningCamera.TransformPoint(rayDirections[i]*6f));
-        currentCost += Vector3.Distance(refPoints[i], inPoints[i]);
+        refPoints[i] = new Vector4(features[i].position.x, features[i].position.y, features[i].position.z, 1f);
+
+        refPoints[i].w = useLinearEstimate ? 1f : Mathf.Min(1f, 1f / (Mathf.Pow((features[i].position - aligningCamera.position).magnitude * 10f, 2f) +
+                                                                      Mathf.Pow((features[i].position - inPoints[i]            ).magnitude * 50f, 2f)));
+
+        currentCost += Vector3.Distance(refPoints[i], inPoints[i]) * refPoints[i].w;
         Debug.DrawLine(aligningCamera.position, aligningCamera.TransformPoint(rayDirections[i]));
         Debug.DrawLine(features[i].position, inPoints[i], Color.red);
       }

diff --git a/Assets/BundleAdustment/StereoBundleAdjuster.cs b/Assets/BundleAdustment/StereoBundleAdjuster.cs
@@ -8,10 +8,10 @@ public class StereoBundleAdjuster : MonoBehaviour {
   Vector3[][] rayDirections;
 
   KabschSolver solver = new KabschSolver();
-  Vector3[] inPoints, refPoints;
+  Vector4[] inPoints; Vector3[] refPoints;
 
   void Start() {
-    inPoints = new Vector3[features.Length * 4];
+    inPoints = new Vector4[features.Length * 4];
     refPoints = new Vector3[features.Length * 4];
     rayDirections = new Vector3[4][];
 
@@ -39,7 +39,7 @@ void Update() {
           Displacement(cameras[(cameraIndex / 2)    ].position, cameras[(cameraIndex / 2)    ].TransformPoint(rayDirections[(cameraIndex / 2)    ][i]),
                        cameras[(cameraIndex % 2) + 2].position, cameras[(cameraIndex % 2) + 2].TransformPoint(rayDirections[(cameraIndex % 2) + 2][i]), out pointLineOne, out pointLineTwo);
 
-          inPoints [(i * cameras.Length) + cameraIndex] = pointLineOne;
+          inPoints[(i * cameras.Length) + cameraIndex] = new Vector4(pointLineOne.x, pointLineOne.y, pointLineOne.z, 1f);// / (pointLineOne - pointLineTwo).sqrMagnitude);
           refPoints[(i * cameras.Length) + cameraIndex] = pointLineTwo;
 
           Debug.DrawLine(pointLineOne, pointLineTwo, Color.red);

diff --git a/Assets/Control/BangBangController.cs b/Assets/Control/BangBangController.cs
@@ -31,7 +31,7 @@ bool bangBangController(float currentPosition, float currentVelocity, float targ
            (currentVelocity >= 0f && currentPosition - target < -0.5f * Mathf.Pow(currentVelocity, 2f) / maxForceDividedByMass);
   }
 
-  int bangBangControllerWithZero(float currentPosition, float currentVelocity, float target, float maxForceDividedByMass, float epsilon = 0.0001f) {
+  int bangBangControllerWithZero(float currentPosition, float currentVelocity, float target, float maxForceDividedByMass, float epsilon = 0.01f) {
     float disparity = currentPosition - target;
     float parabola = 0.5f * Mathf.Pow(currentVelocity, 2f) / maxForceDividedByMass;
     if ((currentVelocity <  0f && disparity + epsilon <= parabola) ||