void RangeImageSpherical::calculate3DPoint (double image_x, double image_y, double range, Eigen::Vector3d& point) const { double angle_x, angle_y; getAnglesFromImagePoint (image_x, image_y, angle_x, angle_y); double cosY = cos (angle_y); point = Eigen::Vector3d (range * sin (angle_x) * cosY, range * sin (angle_y), range * cos (angle_x)*cosY); point = to_world_system_ * point; }
void RangeImageSpherical::calculate3DPoint (float image_x, float image_y, float range, Eigen::Vector3f& point) const { float angle_x, angle_y; getAnglesFromImagePoint (image_x, image_y, angle_x, angle_y); float cosY = cosf (angle_y); point = Eigen::Vector3f (range * sinf (angle_x) * cosY, range * sinf (angle_y), range * cosf (angle_x)*cosY); point = to_world_system_ * point; }