cv::Point2d PinholeCameraModel::unrectifyPoint(const cv::Point2d& uv_rect) const
{
assert( initialized() );
if (cache_->distortion_state == NONE)
return uv_rect;
if (cache_->distortion_state == UNKNOWN)
throw Exception("Cannot call unrectifyPoint when distortion is unknown.");
assert(cache_->distortion_state == CALIBRATED);
// Convert to a ray
cv::Point3d ray = projectPixelTo3dRay(uv_rect);
// Project the ray on the image
cv::Mat r_vec, t_vec = cv::Mat_<double>::zeros(3, 1);
cv::Rodrigues(R_.t(), r_vec);
std::vector<cv::Point2d> image_point;
cv::projectPoints(std::vector<cv::Point3d>(1, ray), r_vec, t_vec, K_, D_, image_point);
return image_point[0];
}
void PinholeCamRayCaster::computeRelRayDirections()
{
ray_directions_->clear();
double min_x = config_.resolution.min_x_perc*config_.img_width_px;
double max_x = config_.resolution.max_x_perc*config_.img_width_px;
double min_y = config_.resolution.min_y_perc*config_.img_height_px;
double max_y = config_.resolution.max_y_perc*config_.img_height_px;
double x_step = 1.0/config_.resolution.ray_resolution_x;
double y_step = 1.0/config_.resolution.ray_resolution_y;
for( double x = min_x; x<=max_x; x+=x_step )
{
for( double y = min_y; y<=max_y; y+=y_step )
{
RayDirection ray_dir = projectPixelTo3dRay(x,y);
ray_directions_->push_back(ray_dir);
}
}
}
void PinholeCameraModel::projectPixelTo3dRay(const cv::Point2d& uv_rect, cv::Point3d& ray) const
{
ray = projectPixelTo3dRay(uv_rect);
}
