The second col of R_align is gravity direction
| 9 | |
| 10 | // The second col of R_align is gravity direction |
| 11 | Eigen::Matrix3d GetAlignRot(const Eigen::Vector3d& gravity) { |
| 12 | Eigen::Matrix3d R; |
| 13 | Eigen::Vector3d v = gravity.normalized(); |
| 14 | R.col(1) = v; |
| 15 | |
| 16 | Eigen::Matrix3d Q = v.householderQr().householderQ(); |
| 17 | Eigen::Matrix<double, 3, 2> N = Q.rightCols(2); |
| 18 | R.col(0) = N.col(0); |
| 19 | R.col(2) = N.col(1); |
| 20 | if (R.determinant() < 0) { |
| 21 | R.col(2) = -R.col(2); |
| 22 | } |
| 23 | return R; |
| 24 | } |
| 25 | |
| 26 | double RotUpToAngle(const Eigen::Matrix3d& R_up) { |
| 27 | return RotationToAngleAxis(R_up)[1]; |