| 152 | } |
| 153 | |
| 154 | bool GeoreferenceFilter::processOne(PointRef& point) |
| 155 | { |
| 156 | georeference::TrajPoint barycenter; |
| 157 | if (!m_config->m_trajectory->getTrajPoint( |
| 158 | point.getFieldAs<double>(Dimension::Id::GpsTime) + |
| 159 | m_config->m_timeOffset, |
| 160 | barycenter)) |
| 161 | return false; |
| 162 | |
| 163 | const Eigen::Affine3d transform( |
| 164 | georeference::Utils::getTransformation( |
| 165 | 0.0, 0.0, 0.0, barycenter.roll, barycenter.pitch, |
| 166 | barycenter.azimuth - barycenter.wanderAngle) * |
| 167 | m_config->m_scan2imu); |
| 168 | |
| 169 | m_localCartesian->reset(georeference::Utils::rad2deg(barycenter.y), |
| 170 | georeference::Utils::rad2deg(barycenter.x), |
| 171 | barycenter.z); |
| 172 | if (m_config->m_reverse) |
| 173 | { |
| 174 | m_localCartesian->forward(point); |
| 175 | const Eigen::Vector3d scan( |
| 176 | transform.inverse() * |
| 177 | (m_config->m_ned |
| 178 | ? Eigen::Vector3d(point.getFieldAs<double>(DimId::Y), |
| 179 | point.getFieldAs<double>(DimId::X), |
| 180 | -point.getFieldAs<double>(DimId::Z)) |
| 181 | : Eigen::Vector3d(point.getFieldAs<double>(DimId::X), |
| 182 | point.getFieldAs<double>(DimId::Y), |
| 183 | point.getFieldAs<double>(DimId::Z)))); |
| 184 | point.setField(DimId::X, scan.x()); |
| 185 | point.setField(DimId::Y, scan.y()); |
| 186 | point.setField(DimId::Z, scan.z()); |
| 187 | |
| 188 | // Transform BeamOrigin (point) - same as X/Y/Z |
| 189 | if (m_config->m_transformBeam) |
| 190 | { |
| 191 | // Create temp point for BeamOrigin transformation |
| 192 | double origX = point.getFieldAs<double>(DimId::BeamOriginX); |
| 193 | double origY = point.getFieldAs<double>(DimId::BeamOriginY); |
| 194 | double origZ = point.getFieldAs<double>(DimId::BeamOriginZ); |
| 195 | double dirX = |
| 196 | origX + point.getFieldAs<double>(DimId::BeamDirectionX); |
| 197 | double dirY = |
| 198 | origY + point.getFieldAs<double>(DimId::BeamDirectionY); |
| 199 | double dirZ = |
| 200 | origZ + point.getFieldAs<double>(DimId::BeamDirectionZ); |
| 201 | m_localCartesian->forward(origX, origY, origZ); |
| 202 | m_localCartesian->forward(dirX, dirY, dirZ); |
| 203 | |
| 204 | // Apply same transformation as for X/Y/Z |
| 205 | const Eigen::Vector3d scanOrig( |
| 206 | transform.inverse() * |
| 207 | (m_config->m_ned ? Eigen::Vector3d(origY, origX, -origZ) |
| 208 | : Eigen::Vector3d(origX, origY, origZ))); |
| 209 | |
| 210 | const Eigen::Vector3d scanDir( |
| 211 | transform.inverse() * |
nothing calls this directly
no test coverage detected