| 150 | } |
| 151 | |
| 152 | bool |
| 153 | PulseCollection::EstimatedPositionVelocity(double t, Eigen::Vector3d& r, Eigen::Vector3d& v) const |
| 154 | { |
| 155 | std::vector<Pulse> psub; |
| 156 | |
| 157 | // Extract pulses in a window around t. Adjust pulse times to be relative to t. |
| 158 | for (const Pulse& p: pulses) |
| 159 | { |
| 160 | if (p.t >= t - m_args.tblock && p.t <= t + m_args.tblock) { |
| 161 | psub.push_back(p); |
| 162 | psub.back().t -= t; |
| 163 | } |
| 164 | } |
| 165 | |
| 166 | int k = int(psub.size()); |
| 167 | |
| 168 | // Accumulate scan vector for scan-angle pulses |
| 169 | int kscan = 0; |
| 170 | double sx, sy; |
| 171 | bool skipscan = false; |
| 172 | |
| 173 | { |
| 174 | Eigen::Vector3d scandir(Eigen::Vector3d::Zero()); |
| 175 | for (int l = 0; l < k; ++l) { |
| 176 | if (!psub[l].MultiReturn()) { |
| 177 | scandir += psub[l].n; |
| 178 | ++kscan; |
| 179 | } |
| 180 | } |
| 181 | sx = -scandir(0); sy = -scandir(1); |
| 182 | double h = std::hypot(sx, sy); |
| 183 | if (h > 0) |
| 184 | { sx /= h; sy /= h; } |
| 185 | else |
| 186 | skipscan = true; |
| 187 | } |
| 188 | |
| 189 | int m = skipscan ? k - kscan : k; |
| 190 | if (m < m_args.estn) |
| 191 | return false; |
| 192 | |
| 193 | // For multi-return pulses |
| 194 | // equation for pulse starting at r in direction n, distance = s |
| 195 | // |
| 196 | // x + vx*t = rx + nx * s |
| 197 | // y + vy*t = ry + ny * s |
| 198 | // z + vz*t = rz + nz * s |
| 199 | // |
| 200 | // replace s by z as parameterization, s = (z + t*vz - rz)/nz |
| 201 | // nz*x - nx*z + nz*t*vx - nx*t*vz = nz*rx - nx*rz |
| 202 | // nz*y - ny*z + nz*t*vy - ny*t*vz = nz*ry - ny*rz |
| 203 | // or |
| 204 | // |
| 205 | // A . [x,y,z,vx,vy,vz]' = B |
| 206 | // |
| 207 | // where |
| 208 | // |
| 209 | // A = [nz, 0, -nx, nz*t, 0, -nx*t] |
nothing calls this directly
no test coverage detected