| 33 | } |
| 34 | |
| 35 | inline Eigen::Matrix3d PerpProjector(const Eigen::Vector3d& v, double d) |
| 36 | { |
| 37 | // Assume v is a unit vector |
| 38 | double v2 = v(0) * v(0) + v(1) * v(1); |
| 39 | Eigen::Matrix3d M; |
| 40 | // See proj.mac for derivation |
| 41 | M(0,0) = (v(0)*v(0) * v(2) + v(1)*v(1)) / v2; |
| 42 | M(1,1) = (v(1)*v(1) * v(2) + v(0)*v(0)) / v2; |
| 43 | M(0,1) = -(1 - v(2)) * v(0) * v(1) / v2; |
| 44 | M(1,0) = M(0,1); |
| 45 | M(0,2) = -v(0); |
| 46 | M(1,2) = -v(1); |
| 47 | M.row(2) = v.transpose(); |
| 48 | // Scale first two rows to get projection error at v |
| 49 | M.row(0) *= d; |
| 50 | M.row(1) *= d; |
| 51 | return M; |
| 52 | } |
| 53 | |
| 54 | } // namespace trajectory |
| 55 | } // namespace pdal |