lankun/sjy01-image-proc
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

相机偏置矩阵

Browse Source
This commit is contained in:
nuknal
2024-09-04 15:15:25 +08:00
parent 884dee3c82
commit abe5ecbdec
5 changed files with 51 additions and 32 deletions
Download Patch File Download Diff File Expand all files Collapse all files

17
pkg/calculator/camera.go
View File

@@ -14,9 +14,10 @@ const (
PANPixels = float64(payload.PAN_PIXEL_WIDTH)
PANCellSize = 3.2 // µm
MSSPixels = float64(payload.MSS_PIXEL_WIDTH)
MSSCellSize = 12.8 // µm
CameraRoll = 0.0 // 相机与卫星本体X轴的安装角度, degree FIXME: 安装矩阵应该由卫星方提供
CameraPitch = 0.0 // 相机与卫星本体Y轴的安装角度, degree
MSSCellSize = 12.8 // µm
CameraRoll = -0.010 // 相机与卫星本体X轴的安装角度, degree FIXME: 安装矩阵应该由卫星方提供
CameraPitch = 0.183 // 相机与卫星本体Y轴的安装角度, degree
CameraYaw = 0.012 // 相机与卫星本体Z轴的安装角度, degree
)
// 计算过程使用PAN分辨率
@@ -80,9 +81,17 @@ func CameraRotMatrix(phi, theta, psi float64) *mat.Dense {
-math.Sin(theta), 0, math.Cos(theta),
})
Rz := mat.NewDense(3, 3, []float64{
math.Cos(psi), -math.Sin(psi), 0,
math.Sin(psi), math.Cos(psi), 0,
0, 0, 1,
})
// R = Rz * Ry * Rx
RyRx := mat.NewDense(3, 3, nil)
RyRx.Mul(Ry, Rx)
Rzxy := mat.NewDense(3, 3, nil)
Rzxy.Mul(Rz, RyRx)
return RyRx
return Rzxy
}

2
pkg/calculator/intersection.go
View File

@@ -23,7 +23,7 @@ func Camera2GroundPoint(q Quaternion, satPos84 []float64, satTime time.Time, uca
direction := CameraDirectionVec(0, float64(ucam))
// -------- 相机坐标系下CCD成像方向向量转到卫星坐标系 --------
Rcam := CameraRotMatrix(CameraRoll*math.Pi/180.0, CameraPitch*math.Pi/180.0, 0)
Rcam := CameraRotMatrix(CameraRoll*math.Pi/180.0, CameraPitch*math.Pi/180.0, CameraYaw*math.Pi/180.0)
var dCam mat.VecDense
dCam.MulVec(Rcam, mat.NewVecDense(3, direction))

4
pkg/producer/rpc.go
View File

@@ -157,7 +157,7 @@ func (rpc *RPC) RPC() error {
// B := setupSystemOfEquations(rowVec, latVec, lonVec, heightVec)
// J, err := SolveNormalEquation(B, rowVec)
log.Println("solving row coefficients")
J, err := solveCoefficients(rowVec, latVec, lonVec, heightVec)
J, err := solveCoefficients(rowVec, latVec, lonVec, heightVec, SolveMethodNelderMead)
if err != nil {
return err
}
@@ -166,7 +166,7 @@ func (rpc *RPC) RPC() error {
// D := setupSystemOfEquations(colVec, latVec, lonVec, heightVec)
// K, err := SolveNormalEquation(D, colVec)
log.Println("solving col coefficients")
K, err := solveCoefficients(colVec, latVec, lonVec, heightVec)
K, err := solveCoefficients(colVec, latVec, lonVec, heightVec, SolveMethodNelderMead)
if err != nil {
return err
}

58
pkg/producer/rpc_helper.go
View File

@@ -14,6 +14,14 @@ type VX struct {
x mat.VecDense
}
type SolveMethod int
const (
SolveMethodOptimize SolveMethod = iota
SolveMethodNelderMead
SolveMethodLeastSquare
)
const epsilon = 1e-4
func normalize(v *mat.VecDense) (*mat.VecDense, float64, float64) {
@@ -35,7 +43,7 @@ func normalize2(v *mat.VecDense, vOff, vScale float64) *mat.VecDense {
return v
}
func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense) ([]float64, error) {
func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense, method SolveMethod) ([]float64, error) {
M := setupSystemOfEquations(f, latVec, lonVec, heightVec)
n := f.Len()
weights := mat.NewDiagDense(n, nil)
@@ -61,27 +69,29 @@ func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense) ([]float64, e
MtW2F.MulVec(&MtW2, f)
x0.MulVec(invMtW2M, &MtW2F)
numerator := mat.NewVecDense(20, nil)
denominator := mat.NewVecDense(20, nil)
denominator.SetVec(0, 1.0)
numerator.SetVec(0, x0.AtVec(0))
for i := 1; i < 20; i++ {
numerator.SetVec(i, x0.AtVec(i))
denominator.SetVec(i, x0.AtVec(i+19))
if method == SolveMethodNelderMead {
numerator := mat.NewVecDense(20, nil)
denominator := mat.NewVecDense(20, nil)
denominator.SetVec(0, 1.0)
numerator.SetVec(0, x0.AtVec(0))
for i := 1; i < 20; i++ {
numerator.SetVec(i, x0.AtVec(i))
denominator.SetVec(i, x0.AtVec(i+19))
}
num, den, err := solveNelderMead(numerator, denominator, f, latVec, lonVec, heightVec)
if err != nil {
return nil, err
}
var coeffs []float64
coeffs = append(coeffs, num.RawVector().Data...)
for i := 1; i < 20; i++ {
coeffs = append(coeffs, den.AtVec(i))
}
return coeffs, nil
}
// num, den, err := solveNelderMead(numerator, denominator, f, latVec, lonVec, heightVec)
// if err != nil {
// return nil, err
// }
// var coeffs []float64
// coeffs = append(coeffs, num.RawVector().Data...)
// for i := 1; i < 20; i++ {
// coeffs = append(coeffs, den.AtVec(i))
// }
// return coeffs, nil
// 迭代
var wm mat.Dense
var wmx, wf mat.VecDense
@@ -91,7 +101,7 @@ func solveCoefficients(f, latVec, lonVec, heightVec *mat.VecDense) ([]float64, e
v0 := 0.0
iterations := 0
maxIterations := 10
denominator := mat.NewVecDense(20, nil)
for iterations < maxIterations {
denominator.SetVec(0, 1.0)
for idx := 1; idx < 20; idx++ {
@@ -243,9 +253,9 @@ func invertRPCMatrix(At *mat.Dense) (*mat.Dense, error) {
if err != nil {
// 岭估计方法调整法方程状态，使得矩阵非奇异，最小二乘平差可以收敛
r, c := At.Dims()
log.Infof("cannot inverse matrix(%d*%d): %v", r, c, err)
log.Debugf("cannot inverse matrix(%d*%d): %v", r, c, err)
k := 0.0000001 // [0.00000005, 0.000005]
log.Infof("try to adjust matrix with +kI, k=%.8f", k)
log.Debugf("try to adjust matrix with +kI, k=%.8f", k)
I := mat.NewDiagDense(r, nil)
for i := 0; i < r; i++ {
I.SetDiag(i, k)
@@ -256,7 +266,7 @@ func invertRPCMatrix(At *mat.Dense) (*mat.Dense, error) {
}
if err != nil {
log.Infof("cannot inverse matrix: %v, try SVD method", err)
log.Debugf("cannot inverse matrix: %v, try SVD method", err)
// 计算矩阵的 SVD 分解
var svd mat.SVD
ok := svd.Factorize(At, mat.SVDThin)

2
pkg/utils/tiff.go
View File

@@ -87,7 +87,7 @@ func SaveBGRToGDALGTiff(bgr gocv.Mat,
defer ds.Close()
// ds.SetMetadata("NBITS", "16")
setGeoTransform(ds, topLeftX, topLeftY, resolution)
// setGeoTransform(ds, topLeftX, topLeftY, resolution)
for b := 0; b < bands; b++ {
band := ds.Bands()[b]