Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
U
Usgscsm
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Package registry
Container registry
Model registry
Operate
Environments
Terraform modules
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
aflab
astrogeology
Usgscsm
Commits
67e0d966
Commit
67e0d966
authored
2 years ago
by
acpaquette
Browse files
Options
Downloads
Patches
Plain Diff
Projected Line Scanner photogrametry updates
parent
936064ff
No related branches found
No related tags found
No related merge requests found
Changes
1
Show whitespace changes
Inline
Side-by-side
Showing
1 changed file
src/UsgsAstroProjectedLsSensorModel.cpp
+11
-339
11 additions, 339 deletions
src/UsgsAstroProjectedLsSensorModel.cpp
with
11 additions
and
339 deletions
src/UsgsAstroProjectedLsSensorModel.cpp
+
11
−
339
View file @
67e0d966
...
...
@@ -289,58 +289,12 @@ csm::ImageCoord UsgsAstroProjectedLsSensorModel::groundToImage(
csm
::
ImageCoordCovar
UsgsAstroProjectedLsSensorModel
::
groundToImage
(
const
csm
::
EcefCoordCovar
&
groundPt
,
double
desired_precision
,
double
*
achieved_precision
,
csm
::
WarningList
*
warnings
)
const
{
MESSAGE_LOG
(
spdlog
::
level
::
debug
,
"Computing groundToImage(Covar) for {}, {}, {}, with desired precision "
"{}"
,
groundPt
.
x
,
groundPt
.
y
,
groundPt
.
z
,
desired_precision
);
// Ground to image with error propagation
// Compute corresponding image point
csm
::
EcefCoord
gp
;
gp
.
x
=
groundPt
.
x
;
gp
.
y
=
groundPt
.
y
;
gp
.
z
=
groundPt
.
z
;
csm
::
ImageCoord
ip
=
groundToImage
(
gp
,
desired_precision
,
achieved_precision
,
warnings
);
csm
::
ImageCoordCovar
result
(
ip
.
line
,
ip
.
samp
);
// // Compute partials ls wrt XYZ
// std::vector<double> prt(6, 0.0);
// prt = UsgsAstroLsSensorModel::computeGroundPartials(groundPt);
// // Error propagation
// double ltx, lty, ltz;
// double stx, sty, stz;
// ltx = prt[0] * groundPt.covariance[0] + prt[1] * groundPt.covariance[3] +
// prt[2] * groundPt.covariance[6];
// lty = prt[0] * groundPt.covariance[1] + prt[1] * groundPt.covariance[4] +
// prt[2] * groundPt.covariance[7];
// ltz = prt[0] * groundPt.covariance[2] + prt[1] * groundPt.covariance[5] +
// prt[2] * groundPt.covariance[8];
// stx = prt[3] * groundPt.covariance[0] + prt[4] * groundPt.covariance[3] +
// prt[5] * groundPt.covariance[6];
// sty = prt[3] * groundPt.covariance[1] + prt[4] * groundPt.covariance[4] +
// prt[5] * groundPt.covariance[7];
// stz = prt[3] * groundPt.covariance[2] + prt[4] * groundPt.covariance[5] +
// prt[5] * groundPt.covariance[8];
// double gp_cov[4]; // Input gp cov in image space
// gp_cov[0] = ltx * prt[0] + lty * prt[1] + ltz * prt[2];
// gp_cov[1] = ltx * prt[3] + lty * prt[4] + ltz * prt[5];
// gp_cov[2] = stx * prt[0] + sty * prt[1] + stz * prt[2];
// gp_cov[3] = stx * prt[3] + sty * prt[4] + stz * prt[5];
// std::vector<double> unmodeled_cov = getUnmodeledError(ip);
// double sensor_cov[4]; // sensor cov in image space
// determineSensorCovarianceInImageSpace(gp, sensor_cov);
// result.covariance[0] = gp_cov[0] + unmodeled_cov[0] + sensor_cov[0];
// result.covariance[1] = gp_cov[1] + unmodeled_cov[1] + sensor_cov[1];
// result.covariance[2] = gp_cov[2] + unmodeled_cov[2] + sensor_cov[2];
// result.covariance[3] = gp_cov[3] + unmodeled_cov[3] + sensor_cov[3];
return
result
;
csm
::
ImageCoordCovar
imageCoordCovar
=
UsgsAstroLsSensorModel
::
groundToImage
(
groundPt
,
desired_precision
,
achieved_precision
,
warnings
);
csm
::
ImageCoord
projImagePt
=
groundToImage
(
groundPt
);
imageCoordCovar
.
line
=
projImagePt
.
line
;
imageCoordCovar
.
samp
=
projImagePt
.
samp
;
return
imageCoordCovar
;
}
//***************************************************************************
...
...
@@ -417,7 +371,10 @@ csm::EcefLocus UsgsAstroProjectedLsSensorModel::imageToProximateImagingLocus(
const
csm
::
ImageCoord
&
image_pt
,
const
csm
::
EcefCoord
&
ground_pt
,
double
desired_precision
,
double
*
achieved_precision
,
csm
::
WarningList
*
warnings
)
const
{
csm
::
ImageCoord
cameraImagePt
=
UsgsAstroLsSensorModel
::
groundToImage
(
ground_pt
);
csm
::
EcefCoord
projGround
=
imageToGround
(
image_pt
,
0
);
csm
::
ImageCoord
cameraImagePt
=
UsgsAstroLsSensorModel
::
groundToImage
(
projGround
);
// std::cout.precision(17);
// std::cout << cameraImagePt.line << ", " <<
return
UsgsAstroLsSensorModel
::
imageToProximateImagingLocus
(
cameraImagePt
,
ground_pt
,
desired_precision
,
achieved_precision
,
warnings
);
}
...
...
@@ -496,69 +453,6 @@ std::string UsgsAstroProjectedLsSensorModel::getReferenceDateAndTime() const {
return
ephemTimeToCalendarTime
(
ephemTime
);
}
//***************************************************************************
// UsgsAstroProjectedLsSensorModel::getImageTime
//***************************************************************************
double
UsgsAstroProjectedLsSensorModel
::
getImageTime
(
const
csm
::
ImageCoord
&
image_pt
)
const
{
// Convert imagept to camera imagept
// proj -> ecef -> groundToImage
MESSAGE_LOG
(
spdlog
::
level
::
debug
,
"getImageTime for image line {}"
,
image_pt
.
line
)
double
lineFull
=
image_pt
.
line
;
auto
referenceLineIt
=
std
::
upper_bound
(
m_intTimeLines
.
begin
(),
m_intTimeLines
.
end
(),
lineFull
);
if
(
referenceLineIt
!=
m_intTimeLines
.
begin
())
{
--
referenceLineIt
;
}
size_t
referenceIndex
=
std
::
distance
(
m_intTimeLines
.
begin
(),
referenceLineIt
);
// Adding 0.5 to the line results in center exposure time for a given line
double
time
=
m_intTimeStartTimes
[
referenceIndex
]
+
m_intTimes
[
referenceIndex
]
*
(
lineFull
-
m_intTimeLines
[
referenceIndex
]
+
0.5
);
MESSAGE_LOG
(
spdlog
::
level
::
debug
,
"getImageTime is {}"
,
time
)
return
time
;
}
//***************************************************************************
// UsgsAstroProjectedLsSensorModel::getSensorPosition
//***************************************************************************
csm
::
EcefCoord
UsgsAstroProjectedLsSensorModel
::
getSensorPosition
(
const
csm
::
ImageCoord
&
imagePt
)
const
{
// Convert imagept to camera imagept
// proj -> ecef -> groundToImage
MESSAGE_LOG
(
spdlog
::
level
::
debug
,
"getSensorPosition at image coord ({}, {})"
,
imagePt
.
line
,
imagePt
.
samp
)
return
UsgsAstroLsSensorModel
::
getSensorPosition
(
getImageTime
(
imagePt
));
}
//***************************************************************************
// UsgsAstroProjectedLsSensorModel::getSensorVelocity
//***************************************************************************
csm
::
EcefVector
UsgsAstroProjectedLsSensorModel
::
getSensorVelocity
(
const
csm
::
ImageCoord
&
imagePt
)
const
{
// Convert imagept to camera imagept
// proj -> ecef -> groundToImage
MESSAGE_LOG
(
spdlog
::
level
::
debug
,
"getSensorVelocity at image coord ({}, {})"
,
imagePt
.
line
,
imagePt
.
samp
);
return
UsgsAstroLsSensorModel
::
getSensorVelocity
(
getImageTime
(
imagePt
));
}
//---------------------------------------------------------------------------
// Sensor Model Parameters
//---------------------------------------------------------------------------
...
...
@@ -712,33 +606,6 @@ UsgsAstroProjectedLsSensorModel::getValidImageRange() const {
// image in a zero based system.
}
//***************************************************************************
// UsgsAstroProjectedLsSensorModel::getIlluminationDirection
//***************************************************************************
csm
::
EcefVector
UsgsAstroProjectedLsSensorModel
::
getIlluminationDirection
(
const
csm
::
EcefCoord
&
groundPt
)
const
{
MESSAGE_LOG
(
spdlog
::
level
::
debug
,
"Accessing illumination direction of ground point"
"{} {} {}."
,
groundPt
.
x
,
groundPt
.
y
,
groundPt
.
z
);
csm
::
EcefVector
sunPosition
=
getSunPosition
(
getImageTime
(
groundToImage
(
groundPt
)));
csm
::
EcefVector
illuminationDirection
=
csm
::
EcefVector
(
groundPt
.
x
-
sunPosition
.
x
,
groundPt
.
y
-
sunPosition
.
y
,
groundPt
.
z
-
sunPosition
.
z
);
double
scale
=
sqrt
(
illuminationDirection
.
x
*
illuminationDirection
.
x
+
illuminationDirection
.
y
*
illuminationDirection
.
y
+
illuminationDirection
.
z
*
illuminationDirection
.
z
);
illuminationDirection
.
x
/=
scale
;
illuminationDirection
.
y
/=
scale
;
illuminationDirection
.
z
/=
scale
;
return
illuminationDirection
;
}
//---------------------------------------------------------------------------
// Error Correction
//---------------------------------------------------------------------------
...
...
@@ -899,201 +766,6 @@ csm::Version UsgsAstroProjectedLsSensorModel::getVersion() const {
return
csm
::
Version
(
1
,
0
,
0
);
}
//***************************************************************************
// UsgsAstroLineScannerSensorModel::getEllipsoid
//***************************************************************************
csm
::
Ellipsoid
UsgsAstroProjectedLsSensorModel
::
getEllipsoid
()
const
{
return
csm
::
Ellipsoid
(
m_majorAxis
,
m_minorAxis
);
}
void
UsgsAstroProjectedLsSensorModel
::
setEllipsoid
(
const
csm
::
Ellipsoid
&
ellipsoid
)
{
m_majorAxis
=
ellipsoid
.
getSemiMajorRadius
();
m_minorAxis
=
ellipsoid
.
getSemiMinorRadius
();
}
//***************************************************************************
// UsgsAstroLineScannerSensorModel::getValue
//***************************************************************************
double
UsgsAstroProjectedLsSensorModel
::
getValue
(
int
index
,
const
std
::
vector
<
double
>&
adjustments
)
const
{
return
m_currentParameterValue
[
index
]
+
adjustments
[
index
];
}
//***************************************************************************
// Functions pulled out of losToEcf and computeViewingPixel
// **************************************************************************
void
UsgsAstroProjectedLsSensorModel
::
getQuaternions
(
const
double
&
time
,
double
q
[
4
])
const
{
int
nOrder
=
8
;
if
(
m_platformFlag
==
0
)
nOrder
=
4
;
int
nOrderQuat
=
nOrder
;
if
(
m_numQuaternions
<
6
&&
nOrder
==
8
)
nOrderQuat
=
4
;
MESSAGE_LOG
(
spdlog
::
level
::
debug
,
"Calculating getQuaternions for time {} with {}"
"order lagrange"
,
time
,
nOrder
)
lagrangeInterp
(
m_numQuaternions
/
4
,
&
m_quaternions
[
0
],
m_t0Quat
,
m_dtQuat
,
time
,
4
,
nOrderQuat
,
q
);
}
//***************************************************************************
// UsgsAstroLineScannerSensorModel::calculateAttitudeCorrection
//***************************************************************************
void
UsgsAstroProjectedLsSensorModel
::
calculateAttitudeCorrection
(
const
double
&
time
,
const
std
::
vector
<
double
>&
adj
,
double
attCorr
[
9
])
const
{
MESSAGE_LOG
(
spdlog
::
level
::
debug
,
"Computing calculateAttitudeCorrection (with adjustment)"
"for time {}"
,
time
)
double
aTime
=
time
-
m_t0Quat
;
double
euler
[
3
];
double
nTime
=
aTime
/
m_halfTime
;
double
nTime2
=
nTime
*
nTime
;
euler
[
0
]
=
(
getValue
(
6
,
adj
)
+
getValue
(
9
,
adj
)
*
nTime
+
getValue
(
12
,
adj
)
*
nTime2
)
/
m_flyingHeight
;
euler
[
1
]
=
(
getValue
(
7
,
adj
)
+
getValue
(
10
,
adj
)
*
nTime
+
getValue
(
13
,
adj
)
*
nTime2
)
/
m_flyingHeight
;
euler
[
2
]
=
(
getValue
(
8
,
adj
)
+
getValue
(
11
,
adj
)
*
nTime
+
getValue
(
14
,
adj
)
*
nTime2
)
/
m_halfSwath
;
MESSAGE_LOG
(
spdlog
::
level
::
trace
,
"calculateAttitudeCorrection: euler {} {} {}"
,
euler
[
0
],
euler
[
1
],
euler
[
2
])
calculateRotationMatrixFromEuler
(
euler
,
attCorr
);
}
//***************************************************************************
// UsgsAstroLineScannerSensorModel::computeProjectiveApproximation
//***************************************************************************
void
UsgsAstroProjectedLsSensorModel
::
computeProjectiveApproximation
(
const
csm
::
EcefCoord
&
gp
,
csm
::
ImageCoord
&
ip
)
const
{
MESSAGE_LOG
(
spdlog
::
level
::
debug
,
"Computing projective approximation for ground point ({}, {}, {})"
,
gp
.
x
,
gp
.
y
,
gp
.
z
);
if
(
m_useApproxInitTrans
)
{
std
::
vector
<
double
>
const
&
u
=
m_projTransCoeffs
;
// alias, to save on typing
double
x
=
gp
.
x
,
y
=
gp
.
y
,
z
=
gp
.
z
;
double
line_den
=
1
+
u
[
4
]
*
x
+
u
[
5
]
*
y
+
u
[
6
]
*
z
;
double
samp_den
=
1
+
u
[
11
]
*
x
+
u
[
12
]
*
y
+
u
[
13
]
*
z
;
// Sanity checks. Ensure we don't divide by 0 and that the numbers are valid.
if
(
line_den
==
0.0
||
std
::
isnan
(
line_den
)
||
std
::
isinf
(
line_den
)
||
samp_den
==
0.0
||
std
::
isnan
(
samp_den
)
||
std
::
isinf
(
samp_den
))
{
ip
.
line
=
m_nLines
/
2.0
;
ip
.
samp
=
m_nSamples
/
2.0
;
MESSAGE_LOG
(
spdlog
::
level
::
warn
,
"Computing initial guess with constant approx line/2 and sample/2"
);
return
;
}
// Apply the formula
ip
.
line
=
(
u
[
0
]
+
u
[
1
]
*
x
+
u
[
2
]
*
y
+
u
[
3
]
*
z
)
/
line_den
;
ip
.
samp
=
(
u
[
7
]
+
u
[
8
]
*
x
+
u
[
9
]
*
y
+
u
[
10
]
*
z
)
/
samp_den
;
MESSAGE_LOG
(
spdlog
::
level
::
debug
,
"Projective approximation before bounding ({}, {})"
,
ip
.
line
,
ip
.
samp
);
// Since this is valid only over the image,
// don't let the result go beyond the image border.
double
numRows
=
m_nLines
;
double
numCols
=
m_nSamples
;
if
(
ip
.
line
<
0.0
)
ip
.
line
=
0.0
;
if
(
ip
.
line
>
numRows
)
ip
.
line
=
numRows
;
if
(
ip
.
samp
<
0.0
)
ip
.
samp
=
0.0
;
if
(
ip
.
samp
>
numCols
)
ip
.
samp
=
numCols
;
}
else
{
ip
.
line
=
m_nLines
/
2.0
;
ip
.
samp
=
m_nSamples
/
2.0
;
}
MESSAGE_LOG
(
spdlog
::
level
::
debug
,
"Projective approximation ({}, {})"
,
ip
.
line
,
ip
.
samp
);
}
//***************************************************************************
// UsgsAstroLineScannerSensorModel::createProjectiveApproximation
//***************************************************************************
void
UsgsAstroProjectedLsSensorModel
::
createProjectiveApproximation
()
{
MESSAGE_LOG
(
spdlog
::
level
::
debug
,
"Calculating createProjectiveApproximation"
);
// Use 9 points (9*4 eventual matrix rows) as we need to fit 14 variables.
const
int
numPts
=
9
;
double
u_factors
[
numPts
]
=
{
0.0
,
0.0
,
0.0
,
0.5
,
0.5
,
0.5
,
1.0
,
1.0
,
1.0
};
double
v_factors
[
numPts
]
=
{
0.0
,
0.5
,
1.0
,
0.0
,
0.5
,
1.0
,
0.0
,
0.5
,
1.0
};
csm
::
EcefCoord
refPt
=
getReferencePoint
();
double
desired_precision
=
0.01
;
double
height
=
computeEllipsoidElevation
(
refPt
.
x
,
refPt
.
y
,
refPt
.
z
,
m_majorAxis
,
m_minorAxis
,
desired_precision
);
if
(
std
::
isnan
(
height
))
{
MESSAGE_LOG
(
spdlog
::
level
::
warn
,
"createProjectiveApproximation: computeElevation of "
"reference point {} {} {} with desired precision "
"{} and major/minor radii {}, {} returned nan height; nonprojective"
,
refPt
.
x
,
refPt
.
y
,
refPt
.
z
,
desired_precision
,
m_majorAxis
,
m_minorAxis
);
m_useApproxInitTrans
=
false
;
return
;
}
MESSAGE_LOG
(
spdlog
::
level
::
trace
,
"createProjectiveApproximation: computeElevation of "
"reference point {} {} {} with desired precision "
"{} and major/minor radii {}, {} returned {} height"
,
refPt
.
x
,
refPt
.
y
,
refPt
.
z
,
desired_precision
,
m_majorAxis
,
m_minorAxis
,
height
);
double
numImageRows
=
m_nLines
;
double
numImageCols
=
m_nSamples
;
std
::
vector
<
csm
::
ImageCoord
>
ip
(
2
*
numPts
);
std
::
vector
<
csm
::
EcefCoord
>
gp
(
2
*
numPts
);
// Sample at two heights above the ellipsoid in order to get a
// reliable estimate of the relationship between image points and
// ground points.
for
(
int
i
=
0
;
i
<
numPts
;
i
++
)
{
ip
[
i
].
line
=
u_factors
[
i
]
*
numImageRows
;
ip
[
i
].
samp
=
v_factors
[
i
]
*
numImageCols
;
gp
[
i
]
=
imageToGround
(
ip
[
i
],
height
);
}
double
delta_z
=
100.0
;
height
+=
delta_z
;
for
(
int
i
=
0
;
i
<
numPts
;
i
++
)
{
ip
[
i
+
numPts
].
line
=
u_factors
[
i
]
*
numImageRows
;
ip
[
i
+
numPts
].
samp
=
v_factors
[
i
]
*
numImageCols
;
gp
[
i
+
numPts
]
=
imageToGround
(
ip
[
i
+
numPts
],
height
);
}
usgscsm
::
computeBestFitProjectiveTransform
(
ip
,
gp
,
m_projTransCoeffs
);
m_useApproxInitTrans
=
true
;
MESSAGE_LOG
(
spdlog
::
level
::
debug
,
"Completed createProjectiveApproximation"
);
}
//***************************************************************************
// UsgsAstroLineScannerSensorModel::constructStateFromIsd
//***************************************************************************
...
...
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment