| 304 | } |
| 305 | |
| 306 | void PulseCollection::Solve() |
| 307 | { |
| 308 | if (usingMulti() && m_multiBuf.size()) |
| 309 | registerMulti(m_multiBuf); |
| 310 | if (usingSingle() && m_singleBuf.size()) |
| 311 | registerSingle(m_singleBuf); |
| 312 | |
| 313 | InitializeTrajectory(); |
| 314 | int num = traj.num; |
| 315 | |
| 316 | ceres::Problem problem; |
| 317 | |
| 318 | // for debugging |
| 319 | int numMultiReturnResidualBlocks = 0; |
| 320 | int numScanAngleResidualBlocks = 0; |
| 321 | |
| 322 | // Set up residual blocks for pulses |
| 323 | for (const Pulse& p : pulses) |
| 324 | { |
| 325 | auto tconv = traj.tconvert(p.t); |
| 326 | int i = tconv.first; |
| 327 | double t = tconv.second; |
| 328 | if (p.MultiReturn()) |
| 329 | { |
| 330 | ceres::CostFunction* cost_function = |
| 331 | new ceres::AutoDiffCostFunction<FirstLastError, |
| 332 | 2, // number of residuals |
| 333 | 3,3,3,3> // data for cubic fit |
| 334 | (new FirstLastError(p, t)); |
| 335 | ceres::LossFunction* loss_function = |
| 336 | new ceres::ScaledLoss(new ceres::CauchyLoss(m_args.dr), |
| 337 | m_args.multiweight, |
| 338 | ceres::TAKE_OWNERSHIP); |
| 339 | problem.AddResidualBlock(cost_function, loss_function, |
| 340 | traj.r[i ].data(), |
| 341 | traj.v[i ].data(), |
| 342 | traj.r[i+1].data(), |
| 343 | traj.v[i+1].data()); |
| 344 | numMultiReturnResidualBlocks++; |
| 345 | } |
| 346 | else |
| 347 | { |
| 348 | ceres::CostFunction* cost_function = |
| 349 | new ceres::AutoDiffCostFunction<ScanAngleError, |
| 350 | 2, // number of residuals |
| 351 | 3,3,3,3, |
| 352 | 2,2,2,2> // data for cubic fit |
| 353 | (new ScanAngleError(p, t, m_args.pitchweight, degreesToRadians(m_args.fixedpitch))); |
| 354 | ceres::LossFunction* loss_function = new ceres::ScaledLoss |
| 355 | (new ceres::CauchyLoss(degreesToRadians(m_args.dang)), m_args.scanweight, |
| 356 | ceres::TAKE_OWNERSHIP); |
| 357 | problem.AddResidualBlock(cost_function, loss_function, |
| 358 | traj.r[i ].data(), |
| 359 | traj.v[i ].data(), |
| 360 | traj.r[i+1].data(), |
| 361 | traj.v[i+1].data(), |
| 362 | attitude.r[i ].data(), |
| 363 | attitude.v[i ].data(), |
no test coverage detected