29#include "boost/math/tools/minima.hpp"
46#define LSST_ScaledPolynomialTransformFitter_TEST_IN_PLACE 0
73 static Keys const it(0);
85 refId(
schema.addField<afw::table::RecordId>(
"ref_id",
"ID of reference object in this match.")),
86 srcId(
schema.addField<afw::table::RecordId>(
"src_id",
"ID of source object in this match.")),
88 "source positions in pixel coordinates.",
"pix")),
89 input(afw::table::Point2DKey::addFields(
schema,
"intermediate",
90 "reference positions in intermediate world coordinates",
92 initial(afw::table::Point2DKey::addFields(
93 schema,
"initial",
"reference positions transformed by initial WCS",
"pix")),
94 model(afw::table::Point2DKey::addFields(
95 schema,
"model",
"result of applying transform to reference positions",
"pix")),
97 afw::table::CovarianceMatrixKey<float, 2>::addFields(
schema,
"src", {
"x",
"y"},
"pix")),
99 "rejected",
"True if the match should be rejected from the fit.")) {}
103 output(afw::table::Point2DKey::addFields(
104 schema,
"output",
"grid output positions in intermediate world coordinates",
"deg")),
105 input(afw::table::Point2DKey::addFields(
schema,
"input",
106 "grid input positions in pixel coordinates.",
"pix")),
107 model(afw::table::Point2DKey::addFields(
108 schema,
"model",
"result of applying transform to input positions",
"deg")) {}
116 for (
auto const &record :
data) {
119 return geom::AffineTransform(
129 double intrinsicScatter) {
133 float var2 = intrinsicScatter * intrinsicScatter;
134 auto initialIwcToSky = getIntermediateWorldCoordsToSky(initialWcs);
135 for (
auto const &match : matches) {
136 auto record = catalog.
addNew();
137 record->set(keys.refId, match.first->getId());
138 record->set(keys.srcId, match.second->getId());
139 record->set(keys.input, initialIwcToSky->applyInverse(match.first->getCoord()));
140 record->set(keys.initial, initialWcs.
skyToPixel(match.first->getCoord()));
141 record->set(keys.output, match.second->getCentroid());
142 record->set(keys.outputErr, match.second->getCentroidErr() + var2 * Eigen::Matrix2f::Identity());
143 record->set(keys.rejected,
false);
146 computeScaling(catalog, keys.input),
147 computeScaling(catalog, keys.output));
151 int maxOrder,
geom::Box2D const &bbox,
int nGridX,
int nGridY,
155 catalog.
reserve(nGridX * nGridY);
158 for (
int iy = 0; iy < nGridY; ++iy) {
159 for (
int ix = 0; ix < nGridX; ++ix) {
161 auto record = catalog.
addNew();
162 record->set(keys.output, point);
163 record->set(keys.input, toInvert(point));
167 computeScaling(catalog, keys.output));
171 Keys
const &keys,
int maxOrder,
172 double intrinsicScatter,
176 _intrinsicScatter(intrinsicScatter),
178 _outputScaling(outputScaling),
180 _vandermonde(
data.size(), detail::computePackedSize(maxOrder)) {
192 for (
int n = 0, j = 0; n <= maxOrder; ++n) {
193 for (
int p = 0, q = n; p <= n; ++p, --q, ++j) {
194 _vandermonde(i, j) = _transform._poly._u[p] * _transform._poly._v[q];
205 if (
order > maxOrder) {
208 (boost::format(
"Order (%d) exceeded maximum order for the fitter (%d)") %
order % maxOrder)
215 for (
auto const &record : _data) {
221 nGood = _data.
size();
226 Eigen::MatrixXd
m = Eigen::MatrixXd::Zero(nGood, packedSize);
228 Eigen::VectorXd vx = Eigen::VectorXd::Zero(nGood);
229 Eigen::VectorXd vy = Eigen::VectorXd::Zero(nGood);
232 Eigen::ArrayXd sxx(nGood);
233 Eigen::ArrayXd syy(nGood);
234 Eigen::ArrayXd sxy(nGood);
241 vx[i2] = output.getX();
242 vy[i2] = output.getY();
243 m.row(i2) = _vandermonde.row(i1).head(packedSize);
245 Eigen::Matrix2d modelErr =
246 outS * _data[i1].
get(_keys.
outputErr).cast<
double>() * outS.adjoint();
247 sxx[i2] = modelErr(0, 0);
248 sxy[i2] = modelErr(0, 1);
249 syy[i2] = modelErr(1, 1);
259 Eigen::ArrayXd fxx = 1.0 / (sxx - sxy.square() / syy);
260 Eigen::ArrayXd fyy = 1.0 / (syy - sxy.square() / sxx);
261 Eigen::ArrayXd fxy = -(sxy / sxx) * fyy;
262#ifdef LSST_ScaledPolynomialTransformFitter_TEST_IN_PLACE
263 assert((sxx * fxx + sxy * fxy).isApproxToConstant(1.0));
264 assert((syy * fyy + sxy * fxy).isApproxToConstant(1.0));
265 assert((sxx * fxy).isApprox(-sxy * fyy));
266 assert((sxy * fxx).isApprox(-syy * fxy));
270 Eigen::MatrixXd h(2 * packedSize, 2 * packedSize);
271 h.topLeftCorner(packedSize, packedSize) =
m.adjoint() * fxx.matrix().asDiagonal() *
m;
272 h.topRightCorner(packedSize, packedSize) =
m.adjoint() * fxy.matrix().asDiagonal() *
m;
273 h.bottomLeftCorner(packedSize, packedSize) = h.topRightCorner(packedSize, packedSize).adjoint();
274 h.bottomRightCorner(packedSize, packedSize) =
m.adjoint() * fyy.matrix().asDiagonal() *
m;
276 Eigen::VectorXd g(2 * packedSize);
277 g.head(packedSize) =
m.adjoint() * (fxx.matrix().asDiagonal() * vx + fxy.matrix().asDiagonal() * vy);
278 g.tail(packedSize) =
m.adjoint() * (fxy.matrix().asDiagonal() * vx + fyy.matrix().asDiagonal() * vy);
281 auto solution = lstsq.getSolution();
283 for (
int n = 0, j = 0; n <=
order; ++n) {
284 for (
int p = 0, q = n; p <= n; ++p, --q, ++j) {
285 _transform._poly._xCoeffs(p, q) = solution[j];
286 _transform._poly._yCoeffs(p, q) = solution[j + packedSize];
292 for (
auto &record : _data) {
293 record.set(_keys.
model, _transform(record.get(_keys.
input)));
300 "Cannot compute intrinsic scatter on fitter initialized with fromGrid.");
302 double newIntrinsicScatter = computeIntrinsicScatter();
303 float varDiff = newIntrinsicScatter * newIntrinsicScatter - _intrinsicScatter * _intrinsicScatter;
304 for (
auto &record : _data) {
307 _intrinsicScatter = newIntrinsicScatter;
308 return _intrinsicScatter;
311double ScaledPolynomialTransformFitter::computeIntrinsicScatter()
const {
317 double directVariance = 0.0;
318 double maxMeasurementVariance = 0.0;
319 double oldIntrinsicVariance = _intrinsicScatter * _intrinsicScatter;
321 for (
auto const &record : _data) {
323 auto delta = record.get(_keys.
output) - record.get(_keys.
model);
324 directVariance += 0.5 * delta.computeSquaredNorm();
329 double ca2 = 0.5 * (cxx + cyy +
std::sqrt(cxx * cxx + cyy * cyy + 4 * cxy * cxy - 2 * cxx * cyy));
330 maxMeasurementVariance =
std::max(maxMeasurementVariance, ca2);
334 directVariance /= nGood;
338 auto logLikelihood = [
this](
double intrinsicVariance) {
341 double varDiff = intrinsicVariance - this->_intrinsicScatter * this->_intrinsicScatter;
343 for (
auto &record : this->_data) {
349 double det = cxx * cyy - cxy * cxy;
350 q += (
x *
x * cyy - 2 *
x *
y * cxy +
y *
y * cxx) / det +
std::log(det);
357 double minIntrinsicVariance =
std::max(0.0, directVariance - maxMeasurementVariance);
360 static constexpr int BITS_REQUIRED = 16;
361 boost::uintmax_t maxIterations = 20;
362 auto result = boost::math::tools::brent_find_minima(logLikelihood, minIntrinsicVariance, directVariance,
363 BITS_REQUIRED, maxIterations);
373 "Cannot reject outliers on fitter initialized with fromGrid.");
378 (boost::format(
"Not enough values (%d) to clip %d.") % _data.
size() % ctrl.
nClipMin).str());
381 for (
auto &record : _data) {
382 Eigen::Matrix2d cov = record.get(_keys.
outputErr).cast<
double>();
383 Eigen::Vector2d d = (record.get(_keys.
output) - record.get(_keys.
model)).asEigen();
384 double r2 = d.dot(cov.inverse() * d);
388 int nClip = 0, nGood = 0;
389 for (
auto iter = rankings.
begin(); iter != cutoff; ++iter) {
390 iter->second->set(_keys.
rejected,
false);
393 for (
auto iter = cutoff; iter != rankings.
end(); ++iter) {
394 iter->second->set(_keys.
rejected,
true);
400 cutoff->second->set(_keys.
rejected,
true);
403 while (nClip > ctrl.
nClipMax && cutoff != rankings.
end()) {
404 cutoff->second->set(_keys.
rejected,
false);
409 if (cutoff != rankings.
end()) {
#define LSST_EXCEPT(type,...)
lsst::geom::Point2D skyToPixel(lsst::geom::SpherePoint const &sky) const
static LeastSquares fromNormalEquations(ndarray::Array< T1, 2, C1 > const &fisher, ndarray::Array< T2, 1, C2 > const &rhs, Factorization factorization=NORMAL_EIGENSYSTEM)
std::shared_ptr< RecordT > const get(size_type i) const
std::shared_ptr< RecordT > addNew()
void reserve(size_type n)
Eigen::Matrix< T, N, N > get(BaseRecord const &record) const override
T getElement(BaseRecord const &record, int i, int j) const
bool isValid() const noexcept
bool isValid() const noexcept
Key< T > getY() const noexcept
Key< T > getX() const noexcept
Key< T > addField(Field< T > const &field, bool doReplace=false)
Control object for outlier rejection in ScaledPolynomialTransformFitter.
int nClipMax
"Never clip more than this many matches." ;
double nSigma
"Number of sigma to clip at." ;
int nClipMin
"Always clip at least this many matches." ;
PointKey< double > Point2DKey
CatalogT< BaseRecord > BaseCatalog
Extent< double, 2 > Extent2D
Point< double, 2 > Point2D
int computePackedSize(int order)
Compute this size of a packed 2-d polynomial coefficient array.
void computePowers(Eigen::VectorXd &r, double x)
Fill an array with integer powers of x, so .