lsst.meas.astrom g28e8fc4f0a+68211c45a7
pessimisticPatternMatcherUtils.cc
Go to the documentation of this file.
1// -*- LSST-C++ -*-
2
3/*
4 * This file is part of meas_astrom.
5 *
6 * Developed for the LSST Data Management System.
7 * This product includes software developed by the LSST Project
8 * (https://www.lsst.org).
9 * See the COPYRIGHT file at the top-level directory of this distribution
10 * for details of code ownership.
11 *
12 * This program is free software: you can redistribute it and/or modify
13 * it under the terms of the GNU General Public License as published by
14 * the Free Software Foundation, either version 3 of the License, or
15 * (at your option) any later version.
16 *
17 * This program is distributed in the hope that it will be useful,
18 * but WITHOUT ANY WARRANTY; without even the implied warranty of
19 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
20 * GNU General Public License for more details.
21 *
22 * You should have received a copy of the GNU General Public License
23 * along with this program. If not, see <https://www.gnu.org/licenses/>.
24 */
25
26#include <cmath>
27#include <Eigen/Dense>
28#include "ndarray/eigen.h"
30
31namespace lsst {
32namespace meas {
33namespace astrom {
34
36 float src_dist, ndarray::Array<float, 1, 1> const& ref_dist_array, float max_dist_rad) {
37 auto itr =
38 std::upper_bound(ref_dist_array.begin(), ref_dist_array.end(), src_dist - max_dist_rad - 1e-16);
39 auto itrEnd =
40 std::upper_bound(ref_dist_array.begin(), ref_dist_array.end(), src_dist + max_dist_rad + 1e-16);
41
42 size_t startIdx = itr - ref_dist_array.begin();
43 size_t endIdx = itrEnd - ref_dist_array.begin();
44 return std::make_pair(startIdx, endIdx);
45}
46
48 ndarray::Array<double, 1, 1> const& src_ctr, ndarray::Array<double, 2, 1> const& src_delta_array,
49 ndarray::Array<double, 1, 1> const& src_dist_array, ndarray::Array<double, 1, 1> const& ref_ctr,
50 ndarray::Array<double, 1, 1> const& proj_ref_ctr_delta,
51 ndarray::Array<float, 1, 1> const& ref_dist_array, ndarray::Array<uint16_t, 1, 1> const& ref_id_array,
52 ndarray::Array<double, 2, 1> const& reference_array, double max_dist_rad, size_t n_match) {
53 // Struct where we will be putting our results.
55
56 // Counter for number of spokes we failed to find a reference
57 // candidate for. We break the loop if we haven't found enough.
58 size_t n_fail = 0;
59
60 // Plane project the center/first spoke of the source pattern using
61 // the center vector of the pattern as normal.
62 auto src_ctr_eigen = ndarray::asEigenMatrix(src_ctr);
63 double src_delta_ctr_dot = ndarray::asEigenMatrix(src_delta_array[0]).dot(src_ctr_eigen);
64
65 ndarray::Array<double, 1, 1> proj_src_ctr_delta = copy(src_delta_array[0] - src_delta_ctr_dot * src_ctr);
66 auto proj_src_ctr_delta_eigen = ndarray::asEigenMatrix(proj_src_ctr_delta);
67 double proj_src_ctr_dist_sq = proj_src_ctr_delta_eigen.dot(proj_src_ctr_delta_eigen);
68
69 // Pre - compute the squared length of the projected reference vector.
70 auto proj_ref_ctr_delta_eigen = ndarray::asEigenMatrix(proj_ref_ctr_delta);
71 double proj_ref_ctr_dist_sq = proj_ref_ctr_delta_eigen.dot(proj_ref_ctr_delta_eigen);
72
73 // Loop over the source pairs.
74 // Value of sin where sin(theta) ~= theta to within 0.1%.
75 double max_sin_tol = 0.0447;
76 for (size_t src_idx = 1; src_idx < src_dist_array.size(); src_idx++) {
77 if (n_fail > src_dist_array.size() - (n_match - 1)) {
78 break;
79 }
80 // Find the reference pairs that include our candidate pattern center
81 // and sort them in increasing delta. Check this first so we don't
82 // compute anything else if no candidates exist.
83 std::pair<size_t, size_t> candidate_range =
84 find_candidate_reference_pair_range(src_dist_array[src_idx], ref_dist_array, max_dist_rad);
85 if (candidate_range.first == candidate_range.second) {
86 n_fail++;
87 continue;
88 }
89
90 // Given our length tolerance we can use it to compute a tolerance
91 // on the angle between our spoke.
92 double src_sin_tol = max_dist_rad / (src_dist_array[src_idx] + max_dist_rad);
93
94 // Test if the small angle approximation will still hold. This is
95 // defined as when sin(theta) ~= theta to within 0.1% of each
96 // other. If the implied opening angle is too large we set it to
97 // the 0.1% threshold.
98 if (src_sin_tol > max_sin_tol) {
99 src_sin_tol = max_sin_tol;
100 }
101
102 // Plane project the candidate source spoke and compute the cosine
103 // and sine of the opening angle.
104 double proj_src_delta_dot = ndarray::asEigenMatrix(src_delta_array[src_idx]).dot(src_ctr_eigen);
105
106 ndarray::Array<double, 1, 1> proj_src_delta =
107 copy(src_delta_array[src_idx] - proj_src_delta_dot * src_ctr);
108 auto proj_src_delta_eigen = ndarray::asEigenMatrix(proj_src_delta);
109 double geom_dist_src = sqrt(proj_src_delta_eigen.dot(proj_src_delta_eigen) * proj_src_ctr_dist_sq);
110
111 // Compute cosine and sine of the delta vector opening angle.
112 double cos_theta_src = proj_src_delta_eigen.dot(proj_src_ctr_delta_eigen) / geom_dist_src;
113 Eigen::Vector3d cross_src =
114 proj_src_delta_eigen.head<3>().cross(proj_src_ctr_delta_eigen.head<3>()) / geom_dist_src;
115 double sin_theta_src = cross_src.dot(src_ctr_eigen);
116
117 // Test the spokes and return the id of the reference object.
118 // Return -1 if no match is found.
119 int ref_id =
120 check_spoke(cos_theta_src, sin_theta_src, ref_ctr, proj_ref_ctr_delta, proj_ref_ctr_dist_sq,
121 candidate_range, ref_id_array, reference_array, src_sin_tol);
122 if (ref_id < 0) {
123 n_fail++;
124 continue;
125 }
126
129 output_spokes.push_back(std::make_pair(static_cast<size_t>(ref_id), src_idx + 1));
130 if (output_spokes.size() >= n_match - 2) {
131 break;
132 }
133 }
134 return output_spokes;
135}
136
137int check_spoke(double cos_theta_src, double sin_theta_src, ndarray::Array<double, 1, 1> const& ref_ctr,
138 ndarray::Array<double, 1, 1> const& proj_ref_ctr_delta, double proj_ref_ctr_dist_sq,
139 std::pair<size_t, size_t> const& candidate_range,
140 ndarray::Array<uint16_t, 1, 1> const& ref_id_array,
141 ndarray::Array<double, 2, 1> const& reference_array, double src_sin_tol) {
142 // Loop over our candidate reference objects. candidate_range is the min
143 // and max of for pair candidates and are view into ref_id_array. Here we
144 // start from the midpoint of min and max values and step outward.
145 size_t midpoint = (candidate_range.first + candidate_range.second) / 2;
146 for (size_t idx = 0; idx < candidate_range.second - candidate_range.first; idx++) {
147 if (idx % 2 == 0) {
148 midpoint = midpoint + idx;
149 } else {
150 midpoint = midpoint - idx;
151 }
152 // Compute the delta vector from the pattern center.
153 uint16_t ref_id = ref_id_array[midpoint];
154 ndarray::Array<double, 1, 1> ref_delta = copy(reference_array[ref_id] - ref_ctr);
155
156 double ref_dot = ndarray::asEigenMatrix(ref_delta).dot(ndarray::asEigenMatrix(ref_ctr));
157 ndarray::Array<double, 1, 1> proj_ref_delta = copy(ref_delta - ref_dot * ref_ctr);
158 // Compute the cos between our "center" reference vector and the
159 // current reference candidate.
160 auto proj_ref_delta_eigen = ndarray::asEigenMatrix(proj_ref_delta);
161 auto proj_ref_ctr_delta_eigen = ndarray::asEigenMatrix(proj_ref_ctr_delta);
162 double proj_delta_dist_sq = proj_ref_delta_eigen.dot(proj_ref_delta_eigen);
163 double geom_dist_ref = sqrt(proj_ref_ctr_dist_sq * proj_delta_dist_sq);
164 double cos_theta_ref = proj_ref_delta_eigen.dot(proj_ref_ctr_delta_eigen) / geom_dist_ref;
165
166 // Make sure we can safely make the comparison in case
167 // our "center" and candidate vectors are mostly aligned.
168 double cos_sq_comparison;
169 if (cos_theta_ref * cos_theta_ref < (1 - src_sin_tol * src_sin_tol)) {
170 cos_sq_comparison = (cos_theta_src - cos_theta_ref) * (cos_theta_src - cos_theta_ref) /
171 (1 - cos_theta_ref * cos_theta_ref);
172 } else {
173 cos_sq_comparison = (cos_theta_src - cos_theta_ref) * (cos_theta_src - cos_theta_ref) /
174 (src_sin_tol * src_sin_tol);
175 }
176 // Test the difference of the cosine of the reference angle against
177 // the source angle. Assumes that the delta between the two is
178 // small.
179 if (cos_sq_comparison > src_sin_tol * src_sin_tol) {
180 continue;
181 }
182 // The cosine tests the magnitude of the angle but not
183 // its direction. To do that we need to know the sine as well.
184 // This cross product calculation does that.
185 Eigen::Vector3d cross_ref =
186 proj_ref_delta_eigen.head<3>().cross(proj_ref_ctr_delta_eigen.head<3>()) / geom_dist_ref;
187 double sin_theta_ref = cross_ref.dot(ndarray::asEigenMatrix(ref_ctr));
188 // Check the value of the cos again to make sure that it is not
189 // near zero.
190 double sin_comparison;
191 if (abs(cos_theta_src) < src_sin_tol) {
192 sin_comparison = (sin_theta_src - sin_theta_ref) / src_sin_tol;
193 } else {
194 sin_comparison = (sin_theta_src - sin_theta_ref) / cos_theta_ref;
195 }
196
197 // Return the correct id of the candidate we found.
198 if (abs(sin_comparison) < src_sin_tol) {
199 return ref_id;
200 }
201 }
202 return -1;
203}
204
205} // namespace astrom
206} // namespace meas
207} // namespace lsst
T make_pair(T... args)
int check_spoke(double cos_theta_src, double sin_theta_src, ndarray::Array< double, 1, 1 > const &ref_ctr, ndarray::Array< double, 1, 1 > const &proj_ref_ctr_delta, double proj_ref_ctr_dist_sq, std::pair< size_t, size_t > const &candidate_range, ndarray::Array< uint16_t, 1, 1 > const &ref_id_array, ndarray::Array< double, 2, 1 > const &reference_array, double src_sin_tol)
Check the opening angle between the first spoke of our pattern for the source object against the refe...
std::pair< size_t, size_t > find_candidate_reference_pair_range(float src_dist, ndarray::Array< float, 1, 1 > const &ref_dist_array, float max_dist_rad)
Find the range of reference spokes within a spoke distance tolerance of our source spoke.
std::vector< std::pair< size_t, size_t > > create_pattern_spokes(ndarray::Array< double, 1, 1 > const &src_ctr, ndarray::Array< double, 2, 1 > const &src_delta_array, ndarray::Array< double, 1, 1 > const &src_dist_array, ndarray::Array< double, 1, 1 > const &ref_ctr, ndarray::Array< double, 1, 1 > const &proj_ref_ctr_delta, ndarray::Array< float, 1, 1 > const &ref_dist_array, ndarray::Array< uint16_t, 1, 1 > const &ref_id_array, ndarray::Array< double, 2, 1 > const &reference_array, double max_dist_rad, size_t n_match)
Create the individual spokes that make up the pattern now that the shift and rotation are within tole...
T push_back(T... args)
T size(T... args)
T upper_bound(T... args)