| 36 | namespace { |
| 37 | |
| 38 | bool FisheyeCameraModelIsValidPixel(const CameraModelId model_id, |
| 39 | const std::vector<double>& params, |
| 40 | const Eigen::Vector2d& xy) { |
| 41 | switch (model_id) { |
| 42 | #define CAMERA_MODEL_CASE(CameraModel) \ |
| 43 | case CameraModel::model_id: { \ |
| 44 | double uu, vv; \ |
| 45 | CameraModel::FisheyeFromImg(params.data(), xy.x(), xy.y(), &uu, &vv); \ |
| 46 | const double theta = std::sqrt(uu * uu + vv * vv); \ |
| 47 | if (theta < EIGEN_PI / 2.0) { \ |
| 48 | return true; \ |
| 49 | } else { \ |
| 50 | return false; \ |
| 51 | } \ |
| 52 | } |
| 53 | |
| 54 | FISHEYE_CAMERA_MODEL_CASES |
| 55 | default: |
| 56 | throw std::domain_error( |
| 57 | "Camera model does not exist or is not a fisheye camera"); |
| 58 | |
| 59 | #undef CAMERA_MODEL_CASE |
| 60 | } |
| 61 | |
| 62 | return false; |
| 63 | } |
| 64 | |
| 65 | template <typename CameraModel> |
| 66 | void TestCamToCamFromImg(const std::vector<double>& params, |