| 528 | } |
| 529 | |
| 530 | void RotationAveragingProblem::UpdateState(const Eigen::VectorXd& step) { |
| 531 | // Update frame rotations. |
| 532 | for (const auto& [frame_id, frame_param_idx] : frame_id_to_param_idx_) { |
| 533 | if (!HasFrameGravity(frame_id)) { |
| 534 | const Eigen::Matrix3d estimated_rig_from_world = |
| 535 | AngleAxisToRotationMatrix( |
| 536 | estimated_rotations_.segment<3>(frame_param_idx)); |
| 537 | estimated_rotations_.segment<3>(frame_param_idx) = |
| 538 | RotationMatrixToAngleAxis( |
| 539 | estimated_rig_from_world * |
| 540 | AngleAxisToRotationMatrix(-step.segment<3>(frame_param_idx))); |
| 541 | } else { |
| 542 | estimated_rotations_[frame_param_idx] -= step[frame_param_idx]; |
| 543 | } |
| 544 | } |
| 545 | |
| 546 | // Compute current frame rotations for cam_from_rig averaging. |
| 547 | std::unordered_map<frame_t, Eigen::Matrix3d> frame_rotations; |
| 548 | for (const auto& [frame_id, frame_param_idx] : frame_id_to_param_idx_) { |
| 549 | if (!HasFrameGravity(frame_id)) { |
| 550 | frame_rotations[frame_id] = AngleAxisToRotationMatrix( |
| 551 | estimated_rotations_.segment<3>(frame_param_idx)); |
| 552 | } else { |
| 553 | frame_rotations[frame_id] = |
| 554 | RotationFromYAxisAngle(estimated_rotations_[frame_param_idx]); |
| 555 | } |
| 556 | } |
| 557 | |
| 558 | // Update the global rotations for cam_from_rig cameras. |
| 559 | // Note: the update is non-trivial, and we need to average the rotations from |
| 560 | // all the frames. |
| 561 | for (const auto& [camera_id, camera_param_idx] : camera_id_to_param_idx_) { |
| 562 | const Eigen::Matrix3d estimated_cam_from_rig = AngleAxisToRotationMatrix( |
| 563 | estimated_rotations_.segment<3>(camera_param_idx)); |
| 564 | const Eigen::Matrix3d R_update = |
| 565 | AngleAxisToRotationMatrix(-step.segment<3>(camera_param_idx)); |
| 566 | |
| 567 | std::vector<Eigen::Quaterniond> rig_rotations; |
| 568 | for (const frame_t frame_id : camera_to_frame_ids_[camera_id]) { |
| 569 | const Eigen::Matrix3d& R = frame_rotations[frame_id]; |
| 570 | rig_rotations.push_back(Eigen::Quaterniond(estimated_cam_from_rig * R * |
| 571 | R_update * R.transpose())); |
| 572 | } |
| 573 | |
| 574 | // Average the rotations for the rig. |
| 575 | const Eigen::Quaterniond R_ave = AverageQuaternions( |
| 576 | rig_rotations, std::vector<double>(rig_rotations.size(), 1)); |
| 577 | estimated_rotations_.segment<3>(camera_param_idx) = |
| 578 | RotationMatrixToAngleAxis(R_ave.toRotationMatrix()); |
| 579 | } |
| 580 | } |
| 581 | |
| 582 | double RotationAveragingProblem::AverageStepSize( |
| 583 | const Eigen::VectorXd& step) const { |
no test coverage detected