Use Brown-Conrady model for distortion

- Remove radialDistortFactor method from Camera
- Rename DistortionCenterOffset to PrincipalPointOffset
- Change back to default simplex size
- Update README.md
- Update example project

README.md

@@ -9,6 +9,8 @@ Additional measurements can be added to increase accuracy.
 
 [Click here to run the current release in the browser!](https://dadido3.github.io/D3surveyor/)
 
+An example project is located [here](https://github.com/Dadido3/D3surveyor/master/example/testscene-1/unoptimized.D3survey).
+
 > :warning: This is a proof-of-concept that works to some extent, but still has many rough edges and limitations.
 
 ![Image showing camera settings and a list of taken photos](/images/example-camera.png) ![Image showing the points mapping editor](/images/example-camera-photo.png)
@@ -23,8 +25,8 @@ You need
 
 Here are the basic steps of how to achieve good (or any) results:
 
-1. Create a new camera object and set its `Angle of view` to match coarsely the long side angle of view of your images you gonna take.
-2. Lock the `Angle of view` parameter.
+1. Create a new camera object and set its `Horizontal angle of view` to match coarsely the long side angle of view of your images you gonna take.
+2. Lock the `Horizontal angle of view` parameter.
 3. Add new photos to the camera object.
    Either import previously taken images, or directly capture new ones on your phone.
    The images should contain as many points of interest as possible, and they should be taken from different positions and perspectives.
@@ -42,9 +44,9 @@ Here are the basic steps of how to achieve good (or any) results:
 
 ## Useful information
 
-- Keep the `Angle of view` parameter of cameras locked, because if the software can't find a good matching solution it will find the trivial solution.
+- Keep the `Horizontal angle of view` parameter of cameras locked, because if the software can't find a good matching solution it will find the trivial solution.
   That is all cameras floating into infinity while the the angle of view approaches 0.
-  Once there is a good solution, you can unlock it to let the optimizer find a better `Angle of view`.
+  Once there is a good solution, you can unlock it to let the optimizer find a better `Horizontal angle of view`.
 - If you already have a good network of points and want to add an additional photo, you only need to add 3 point mappings (flags) to let the optimizer find the photo's origin and orientation.
   Once the photo is correctly aligned in the 3D space, the software will show suggested point mappings that can be confirmed by double clicking on them.
 
@@ -106,4 +108,5 @@ There are many possible features that could be added in the future:
 - Optimizer improvements.
 - List problems like measurements with a high sr.
 - Give user suggestions for a better result.
+- Optimize single entity only.
 - Documentation

camera-photo.go

@@ -215,6 +215,10 @@ func (cp *CameraPhoto) ResidualSqr() float64 {
 func (cp *CameraPhoto) Project(worldCoordinates []Coordinate) []PixelCoordinate {
 	camera := cp.camera
 
+	k1, k2, k3, k4 := float64(camera.DistortionKs[0]), float64(camera.DistortionKs[1]), float64(camera.DistortionKs[2]), float64(camera.DistortionKs[3])
+	p1, p2, p3, p4 := float64(camera.DistortionPs[0]), float64(camera.DistortionPs[1]), float64(camera.DistortionPs[2]), float64(camera.DistortionPs[3])
+	b1, b2 := camera.DistortionBs[0], camera.DistortionBs[1]
+
 	imageCenter := cp.imageSize.Scaled(0.5)
 
 	focalLength := imageCenter.X().Pixels() / math.Tan(camera.HorizontalAOV.Radian()/2)
@@ -235,11 +239,25 @@ func (cp *CameraPhoto) Project(worldCoordinates []Coordinate) []PixelCoordinate
 			PixelDistance(loc4[2]),
 		}
 
-		// Radially distort the coordinates.
 		radiusSqr := localCoordinate.LengthSqr()
-		distortedCoordinate := localCoordinate.Scaled(camera.radialDistortFactor(radiusSqr))
 
-		projectedCoordinates[i] = imageCenter.Add(camera.DistortionCenterOffset).Add(distortedCoordinate.Scaled(focalLength))
+		// Radial distortion.
+		distortedCoordinate := localCoordinate.Scaled(1 + k1*radiusSqr + k2*radiusSqr*radiusSqr + k3*radiusSqr*radiusSqr*radiusSqr + k4*radiusSqr*radiusSqr*radiusSqr*radiusSqr)
+		// Tangential distortion.
+		lx, ly := localCoordinate.X().Pixels(), localCoordinate.Y().Pixels()
+		lxSqr, lySqr := lx*lx, ly*ly
+		lxy := lx * ly
+		p3p4 := PixelDistance(1 + p3*radiusSqr + p4*radiusSqr*radiusSqr)
+		distortedCoordinate = distortedCoordinate.Add(PixelCoordinate{
+			PixelDistance(p1*(radiusSqr+2*lxSqr)+2*p2*lxy) * p3p4,
+			PixelDistance(p2*(radiusSqr+2*lySqr)+2*p1*lxy) * p3p4,
+		})
+
+		// Transformation into image space and last distortion.
+		imgCoordinate := imageCenter.Add(camera.PrincipalPointOffset).Add(distortedCoordinate.Scaled(focalLength))
+		imgCoordinate[0] += distortedCoordinate.X()*b1 + distortedCoordinate.Y()*b2
+
+		projectedCoordinates[i] = imgCoordinate
 	}
 
 	return projectedCoordinates

camera.go

@@ -31,7 +31,7 @@ import (
 )
 
 // Amount of camera distortion coefficients.
-const CameraDistortionKs = 2
+const CameraDistortionKs, CameraDistortionPs, CameraDistortionBs = 4, 4, 2
 
 type Camera struct {
 	vgrouter.NavigatorRef `json:"-"`
@@ -48,11 +48,17 @@ type Camera struct {
 	HorizontalAOVLocked bool  // Prevent the value from being optimized.
 
 	// Lens distortion model parameters.
-
-	DistortionCenterOffset       PixelCoordinate                    // Image center offset measured from the real image center. (Offset of the principal point)
-	DistortionCenterOffsetLocked bool                               // Locked state of the image center offset.
-	DistortionKs                 [CameraDistortionKs]TweakableFloat // List of distortion coefficients.
-	DistortionKsLocked           [CameraDistortionKs]bool           // Locked state of the distortion coefficients.
+	// We will the Brown-Conrady model with the transformation direction from undistorted to distorted.
+	// This is similar to what OpenCV uses, see: https://docs.opencv.org/3.4/d9/d0c/group__calib3d.html
+
+	PrincipalPointOffset       PixelCoordinate
+	PrincipalPointOffsetLocked bool
+	DistortionKs               [CameraDistortionKs]TweakableFloat // List of radial distortion coefficients.
+	DistortionKsLocked         [CameraDistortionKs]bool           // Locked state of the distortion coefficients.
+	DistortionPs               [CameraDistortionPs]TweakableFloat // List of tangential distortion coefficients.
+	DistortionPsLocked         [CameraDistortionPs]bool           // Locked state of the distortion coefficients.
+	DistortionBs               [CameraDistortionBs]PixelDistance  // List of affinity and non-orthogonality distortion coefficients.
+	DistortionBsLocked         [CameraDistortionBs]bool           // Locked state of the distortion coefficients.
 
 	Photos map[string]*CameraPhoto
 }
@@ -71,8 +77,10 @@ func (c *Camera) initData() {
 	c.CreatedAt = time.Now()
 	c.PixelAccuracy = 100
 	c.HorizontalAOV = 70 * 2 * math.Pi / 360 // Start with a guess of 70 deg for AOV.
-	c.DistortionCenterOffsetLocked = true
-	c.DistortionKsLocked = [2]bool{true, true}
+	c.PrincipalPointOffsetLocked = true
+	c.DistortionKsLocked = [4]bool{true, true, true, true}
+	c.DistortionPsLocked = [4]bool{true, true, true, true}
+	c.DistortionBsLocked = [2]bool{true, true}
 	c.Photos = map[string]*CameraPhoto{}
 }
 
@@ -103,10 +111,14 @@ func (c *Camera) Copy(newParent *Site, newKey string) *Camera {
 	copy.PixelAccuracy = c.PixelAccuracy
 	copy.HorizontalAOV = c.HorizontalAOV
 	copy.HorizontalAOVLocked = c.HorizontalAOVLocked
-	copy.DistortionCenterOffset = c.DistortionCenterOffset
-	copy.DistortionCenterOffsetLocked = c.DistortionCenterOffsetLocked
+	copy.PrincipalPointOffset = c.PrincipalPointOffset
+	copy.PrincipalPointOffsetLocked = c.PrincipalPointOffsetLocked
 	copy.DistortionKs = c.DistortionKs
 	copy.DistortionKsLocked = c.DistortionKsLocked
+	copy.DistortionPs = c.DistortionPs
+	copy.DistortionPsLocked = c.DistortionPsLocked
+	copy.DistortionBs = c.DistortionBs
+	copy.DistortionBsLocked = c.DistortionBsLocked
 
 	// Generate copies of all children.
 	for k, v := range c.Photos {
@@ -169,9 +181,9 @@ func (c *Camera) GetTweakablesAndResiduals() ([]Tweakable, []Residualer) {
 		tweakables = append(tweakables, &c.HorizontalAOV)
 	}
 
-	if !c.DistortionCenterOffsetLocked {
-		tweakables = append(tweakables, &c.DistortionCenterOffset[0])
-		tweakables = append(tweakables, &c.DistortionCenterOffset[1])
+	if !c.PrincipalPointOffsetLocked {
+		tweakables = append(tweakables, &c.PrincipalPointOffset[0])
+		tweakables = append(tweakables, &c.PrincipalPointOffset[1])
 	}
 
 	for i, locked := range c.DistortionKsLocked {
@@ -187,14 +199,6 @@ func (c *Camera) GetTweakablesAndResiduals() ([]Tweakable, []Residualer) {
 	return tweakables, residuals
 }
 
-// radialDistortFactor returns a radial scaling factor for the given undistorted (quared) radius.
-// Any undistorted coordinate scaled with this factor around the distortion center will result in an distorted coordinate.
-func (c *Camera) radialDistortFactor(radiusSqr float64) float64 {
-	k1, k2 := float64(c.DistortionKs[0]), float64(c.DistortionKs[1])
-
-	return 1 + k1*radiusSqr + k2*radiusSqr*radiusSqr
-}
-
 // PhotosSorted returns the photos of the camera as a list sorted by date.
 // TODO: Replace with generics once they are available. It's one of the few cases where they are really needed
 func (s *Camera) PhotosSorted() []*CameraPhoto {

camera.vugu

@@ -22,12 +22,28 @@
 			<main:GeneralInputComponent InputType="number" :BindValue="&c.DistortionKs[0]" :BindLocked="&c.DistortionKsLocked[0]"></main:GeneralInputComponent>
 			<label>K2</label>
 			<main:GeneralInputComponent InputType="number" :BindValue="&c.DistortionKs[1]" :BindLocked="&c.DistortionKsLocked[1]"></main:GeneralInputComponent>
+			<label>K3</label>
+			<main:GeneralInputComponent InputType="number" :BindValue="&c.DistortionKs[2]" :BindLocked="&c.DistortionKsLocked[2]"></main:GeneralInputComponent>
+			<label>K4</label>
+			<main:GeneralInputComponent InputType="number" :BindValue="&c.DistortionKs[3]" :BindLocked="&c.DistortionKsLocked[3]"></main:GeneralInputComponent>
+			<label>P1</label>
+			<main:GeneralInputComponent InputType="number" :BindValue="&c.DistortionPs[0]" :BindLocked="&c.DistortionPsLocked[0]"></main:GeneralInputComponent>
+			<label>P2</label>
+			<main:GeneralInputComponent InputType="number" :BindValue="&c.DistortionPs[1]" :BindLocked="&c.DistortionPsLocked[1]"></main:GeneralInputComponent>
+			<label>P3</label>
+			<main:GeneralInputComponent InputType="number" :BindValue="&c.DistortionPs[2]" :BindLocked="&c.DistortionPsLocked[2]"></main:GeneralInputComponent>
+			<label>P4</label>
+			<main:GeneralInputComponent InputType="number" :BindValue="&c.DistortionPs[3]" :BindLocked="&c.DistortionPsLocked[3]"></main:GeneralInputComponent>
+			<label>B1</label>
+			<main:GeneralInputComponent InputType="number" :BindValue="&c.DistortionBs[0]" :BindLocked="&c.DistortionBsLocked[0]"></main:GeneralInputComponent>
+			<label>B2</label>
+			<main:GeneralInputComponent InputType="number" :BindValue="&c.DistortionBs[1]" :BindLocked="&c.DistortionBsLocked[1]"></main:GeneralInputComponent>
 			<div class="w3-card">
 				<div class="w3-container w3-green w3-large" style="padding-bottom: 7px;">
 					Distortion center offset (pixels)
-					<main:ToggleInputComponent LabelText="Lock" :BindValue="&c.DistortionCenterOffsetLocked"></main:ToggleInputComponent>
+					<main:ToggleInputComponent LabelText="Lock" :BindValue="&c.PrincipalPointOffsetLocked"></main:ToggleInputComponent>
 				</div>
-				<main:PixelCoordinateComponent :Editable="true" :HideZ="true" :BindValue="&c.DistortionCenterOffset"></main:PixelCoordinateComponent>
+				<main:PixelCoordinateComponent :Editable="true" :HideZ="true" :BindValue="&c.PrincipalPointOffset"></main:PixelCoordinateComponent>
 			</div>
 		</div>
 	</div>

optimizer.go

@@ -153,7 +153,7 @@ func Optimize(site *Site, stopFunc func() bool) error {
 	}
 
 	//res, err := optimize.Minimize(p, init, nil, &optimize.CmaEsChol{InitStepSize: 0.01})
-	res, err := optimize.Minimize(p, init, &optimize.Settings{Converger: &optimize.FunctionConverge{Absolute: 1e-10, Iterations: 100000}}, &optimize.NelderMead{SimplexSize: 10})
+	res, err := optimize.Minimize(p, init, &optimize.Settings{Converger: &optimize.FunctionConverge{Absolute: 1e-10, Iterations: 100000}}, &optimize.NelderMead{})
 	if err != nil {
 		log.Printf("Optimization failed: %v", err)
 	}