diff --git a/aruco_pose/src/draw.cpp b/aruco_pose/src/draw.cpp index dfb155a2..d8e7b021 100644 --- a/aruco_pose/src/draw.cpp +++ b/aruco_pose/src/draw.cpp @@ -8,22 +8,6 @@ using namespace cv; using namespace cv::aruco; - -/** - * Project points to camera view, preserving their Z coordinate. - * - * @param points Source points in 3D space, in world coordinates. - * @param rvec Rodrigues rotation vector from world to camera frame. - * @param tvec Translation vector from world to camera frame. - * @param cameraMatrix Camera intrinsic matrix. - * @param distCoeffs Distortion coefficients, if any. - * @return Projected point coordinates in camera frame. - */ -static std::vector projectPoints3D(const std::vector& points, - const cv::Mat& rvec, const cv::Mat& tvec, - const cv::Mat& cameraMatrix, - const cv::Mat& distCoeffs); - void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize, int borderBits, bool drawAxis) { @@ -333,37 +317,6 @@ static std::vector lineClip(Point3f p1, Point3f p2, float clipPlaneDist // Unreachable? } -/* Draw a (potentially partially visible) line. */ -static void linePartial(InputOutputArray image, Point3f p1, Point3f p2, const Scalar& color, - int thickness = 1, int lineType = LINE_8, int shift = 0) -{ - // If both points are behind the screen, don't draw anything - if (p1.z <= 0 && p2.z <= 0) { - return; - } - Point2f p1p{p1.x, p1.y}; - Point2f p2p{p2.x, p2.y}; - // If points are on the different sides of the plane, compute intersection point - if (p1.z * p2.z < 0) { - // Compute intersection point with the screen - // We denote alpha as such: - // xi = (1 - alpha) * x1 + alpha * x2 - // yi = (1 - alpha) * y1 + alpha * y2 - // zi = (1 - alpha) * z1 + alpha * z2 = 0 - // Thus, alpha can be expressed as - // alpha = z1 / (z1 - z2) - float alpha = p1.z / (p1.z - p2.z); - Point2f pi{(1 - alpha) * p1.x + alpha * p2.x, (1 - alpha) * p1.y + alpha * p2.y}; - // Now, if z1 is negative, we draw the line from (xi, yi) to (x2, y2), else we draw from (x1, y1) to (xi, yi) - if (p1.z < 0) { - p1p = pi; - } else { - p2p = pi; - } - } - line(image, p1p, p2p, color, thickness, lineType, shift); -} - void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs, InputArray _rvec, InputArray _tvec, float length) {