diff --git a/include/Ctain/Matrix.h b/include/Ctain/Matrix.h index 848a7f8..a3ff95b 100644 --- a/include/Ctain/Matrix.h +++ b/include/Ctain/Matrix.h @@ -190,7 +190,7 @@ namespace Ctain { Matrix<_Scalar> topRightCorner() const { Matrix<_Scalar> sub; sub = *this; - sub.setSub(_Rows-1-Rows, _Cols-1-Cols, Rows, Cols, data); + sub.setSub(0, _Cols-Cols, Rows, Cols, data); return sub; } @@ -340,7 +340,7 @@ namespace Ctain { Matrix<_Scalar> res(_Rows, m._Cols); for(int i = 0; i < _Rows; i++) { for(int j = 0; j < m._Cols; j++) { - int sum = 0; + _Scalar sum = 0; for(int k = 0; k < _Cols; k++) { sum += cData(i, k) * m.cData(k, j); } @@ -383,7 +383,7 @@ namespace Ctain { double sum = 0; for(int i = 0; i < _Rows; i++) { for(int j = 0; j < _Cols; j++) { - sum += Matrix::cData(i, j); + sum += Matrix::cData(i, j) * Matrix::cData(i, j); } } sum = sqrt(sum); diff --git a/src/mynteye/api/camera_models/camera.cc b/src/mynteye/api/camera_models/camera.cc index ac4274c..43041ca 100644 --- a/src/mynteye/api/camera_models/camera.cc +++ b/src/mynteye/api/camera_models/camera.cc @@ -97,46 +97,23 @@ const cv::Mat &Camera::mask(void) const { return m_mask; } + void Camera::estimateExtrinsics( const std::vector &objectPoints, const std::vector &imagePoints, cv::Mat &rvec, cv::Mat &tvec) const { std::vector Ms(imagePoints.size()); for (size_t i = 0; i < Ms.size(); ++i) { - Eigen::Vector3d P; - - liftProjective( - Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P); - - P = P / P(2); + // Eigen::Vector3d P; + Ctain::Vectord P(3, 1), p(2, 1); + p<< imagePoints.at(i).x << imagePoints.at(i).y; - Ms.at(i).x = P(0); - Ms.at(i).y = P(1); - } - - // assume unit focal length, zero principal point, and zero distortion - cv::solvePnP( - objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec); -} - -//subEigen -void Camera::estimateExtrinsics2( - const std::vector &objectPoints, - const std::vector &imagePoints, cv::Mat &rvec, - cv::Mat &tvec) const { - std::vector Ms(imagePoints.size()); - for (size_t i = 0; i < Ms.size(); ++i) { - // Eigen::Vector3d P; - Ctain::Vectord P(3, 1), p(2, 1); - p<< imagePoints.at(i).x << imagePoints.at(i).y; - - // liftProjective( - // Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P); - liftProjective(p, P); - P = P / P(2); - - Ms.at(i).x = P(0); - Ms.at(i).y = P(1); + // liftProjective( + // Eigen::Vector2d(imagePoints.at(i).x, imagePoints.at(i).y), P); + liftProjective(p, P); + P = P / P(2); + Ms.at(i).x = P(0); + Ms.at(i).y = P(1); } // assume unit focal length, zero principal point, and zero distortion @@ -145,19 +122,8 @@ void Camera::estimateExtrinsics2( } double Camera::reprojectionDist( - const Eigen::Vector3d &P1, const Eigen::Vector3d &P2) const { - Eigen::Vector2d p1, p2; - - spaceToPlane(P1, p1); - spaceToPlane(P2, p2); - - return (p1 - p2).norm(); -} - -//subEigen - double Camera::reprojectionDist( - const Ctain::Vectord &P1, const Ctain::Vectord &P2) const { - Ctain::Vectord p1(2, 1), p2(2, 1); + const Ctain::Vector3d &P1, const Ctain::Vector3d &P2) const { + Ctain::Vector2d p1(2, 1), p2(2, 1); spaceToPlane(P1, p1); spaceToPlane(P2, p2); @@ -204,25 +170,13 @@ double Camera::reprojectionError( return totalErr / pointsSoFar; } -double Camera::reprojectionError( - const Eigen::Vector3d &P, const Eigen::Quaterniond &camera_q, - const Eigen::Vector3d &camera_t, const Eigen::Vector2d &observed_p) const { - Eigen::Vector3d P_cam = camera_q.toRotationMatrix() * P + camera_t; - - Eigen::Vector2d p; - spaceToPlane(P_cam, p); - - return (p - observed_p).norm(); -} - -//subEigen double Camera::reprojectionError( const Ctain::Vector3d &P, const Ctain::Quaterniond &camera_q, const Ctain::Vector3d &camera_t, const Ctain::Vector2d &observed_p) const { Ctain::Vector3d P_cam; P_cam = camera_q.toRotationMatrix() * P + camera_t; - Ctain::Vector2d p, res; + Ctain::Vector2d p(2, 1), res(2, 1); spaceToPlane(P_cam, p); res = p - observed_p; return res.norm(); @@ -238,41 +192,6 @@ void Camera::projectPoints( cv::Mat R0; cv::Rodrigues(rvec, R0); - Eigen::MatrixXd R(3, 3); - R << R0.at(0, 0), R0.at(0, 1), R0.at(0, 2), - R0.at(1, 0), R0.at(1, 1), R0.at(1, 2), - R0.at(2, 0), R0.at(2, 1), R0.at(2, 2); - - Eigen::Vector3d t; - t << tvec.at(0), tvec.at(1), tvec.at(2); - - for (size_t i = 0; i < objectPoints.size(); ++i) { - const cv::Point3f &objectPoint = objectPoints.at(i); - - // Rotate and translate - Eigen::Vector3d P; - P << objectPoint.x, objectPoint.y, objectPoint.z; - - P = R * P + t; - - Eigen::Vector2d p; - spaceToPlane(P, p); - - imagePoints.push_back(cv::Point2f(p(0), p(1))); - } -} - -// subEigen -void Camera::projectPoints2( - const std::vector &objectPoints, const cv::Mat &rvec, - const cv::Mat &tvec, std::vector &imagePoints) const { - // project 3D object points to the image plane - imagePoints.reserve(objectPoints.size()); - - // double - cv::Mat R0; - cv::Rodrigues(rvec, R0); - Ctain::MatrixXd R(3, 3); R << R0.at(0, 0) << R0.at(0, 1) << R0.at(0, 2) << R0.at(1, 0) << R0.at(1, 1) << R0.at(1, 2) << @@ -290,7 +209,7 @@ void Camera::projectPoints2( P = R * P + t; - Ctain::Vector2d p; + Ctain::Vector2d p(2, 1); spaceToPlane(P, p); imagePoints.push_back(cv::Point2f(p(0), p(1))); diff --git a/src/mynteye/api/camera_models/camera.h b/src/mynteye/api/camera_models/camera.h index dc51a86..4745a24 100644 --- a/src/mynteye/api/camera_models/camera.h +++ b/src/mynteye/api/camera_models/camera.h @@ -16,7 +16,6 @@ #include #include -#include "eigen3/Eigen/Dense" #include "Ctain/CtainBase.h" #include @@ -28,12 +27,10 @@ namespace models { class Camera { public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW enum ModelType { KANNALA_BRANDT, MEI, PINHOLE, SCARAMUZZA }; class Parameters { public: - EIGEN_MAKE_ALIGNED_OPERATOR_NEW explicit Parameters(ModelType modelType); Parameters( @@ -72,39 +69,16 @@ class Camera { const std::vector > &objectPoints, const std::vector > &imagePoints) = 0; -// subEigen - virtual void estimateIntrinsics2( - const cv::Size &boardSize, - const std::vector > &objectPoints, - const std::vector > &imagePoints) = 0; - virtual void estimateExtrinsics( - const std::vector &objectPoints, - const std::vector &imagePoints, - cv::Mat &rvec, cv::Mat &tvec) const; // NOLINT - - virtual void estimateExtrinsics2( const std::vector &objectPoints, const std::vector &imagePoints, cv::Mat &rvec, cv::Mat &tvec) const; // NOLINT // Lift points from the image plane to the projective space - virtual void liftProjective( - const Eigen::Vector2d &p, Eigen::Vector3d &P) const = 0; // NOLINT - // %output P - -//subEigen - // Lift points from the image plane to the projective space virtual void liftProjective( const Ctain::Vector2d &p, Ctain::Vector3d &P) const = 0; // NOLINT // %output P - // Projects 3D points to the image plane (Pi function) - virtual void spaceToPlane( - const Eigen::Vector3d &P, Eigen::Vector2d &p) const = 0; // NOLINT - // %output p - -//subEigen // Projects 3D points to the image plane (Pi function) virtual void spaceToPlane( const Ctain::Vector3d &P, Ctain::Vector2d &p) const = 0; // NOLINT @@ -139,11 +113,7 @@ class Camera { * \return euclidean distance in the plane */ double reprojectionDist( - const Eigen::Vector3d &P1, const Eigen::Vector3d &P2) const; - -//subEigen - double reprojectionDist( - const Ctain::Vector3d &P1, const Ctain::Vector2d &P2) const; + const Ctain::Vector3d &P1, const Ctain::Vector3d &P2) const; double reprojectionError( const std::vector > &objectPoints, @@ -151,26 +121,13 @@ class Camera { const std::vector &rvecs, const std::vector &tvecs, cv::OutputArray perViewErrors = cv::noArray()) const; - double reprojectionError( - const Eigen::Vector3d &P, const Eigen::Quaterniond &camera_q, - const Eigen::Vector3d &camera_t, const Eigen::Vector2d &observed_p) const; - -//subEigen -double reprojectionError( const Ctain::Vector3d &P, const Ctain::Quaterniond &camera_q, const Ctain::Vector3d &camera_t, const Ctain::Vector2d &observed_p) const; - void projectPoints( const std::vector &objectPoints, const cv::Mat &rvec, - const cv::Mat &tvec, std::vector &imagePoints) const; // NOLINT - -//subEigen - void projectPoints2( - const std::vector &objectPoints, const cv::Mat &rvec, - const cv::Mat &tvec, std::vector &imagePoints) const; // NOLINT - + const cv::Mat &tvec, std::vector &imagePoints) const; protected: cv::Mat m_mask; }; diff --git a/src/mynteye/api/camera_models/equidistant_camera.cc b/src/mynteye/api/camera_models/equidistant_camera.cc index bdfd819..2c899af 100644 --- a/src/mynteye/api/camera_models/equidistant_camera.cc +++ b/src/mynteye/api/camera_models/equidistant_camera.cc @@ -270,102 +270,6 @@ void EquidistantCamera::estimateIntrinsics( params.u0() = u0; params.v0() = v0; - // Initialize focal length - // C. Hughes, P. Denny, M. Glavin, and E. Jones, - // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point - // Extraction, PAMI 2010 - // Find circles from rows of chessboard corners, and for each pair - // of circles, find vanishing points: v1 and v2. - // f = ||v1 - v2|| / PI; - double f0 = 0.0; - for (size_t i = 0; i < imagePoints.size(); ++i) { - std::vector center(boardSize.height); - int arrayLength = boardSize.height; - double *radius = new double[arrayLength]; - memset(radius, 0, arrayLength * sizeof(double)); - for (int r = 0; r < boardSize.height; ++r) { - std::vector circle; - for (int c = 0; c < boardSize.width; ++c) { - circle.push_back(imagePoints.at(i).at(r * boardSize.width + c)); - } - - fitCircle(circle, center[r](0), center[r](1), radius[r]); - } - - for (int j = 0; j < boardSize.height; ++j) { - for (int k = j + 1; k < boardSize.height; ++k) { - // find distance between pair of vanishing points which - // correspond to intersection points of 2 circles - std::vector ipts; - ipts = intersectCircles( - center[j](0), center[j](1), radius[j], center[k](0), center[k](1), - radius[k]); - - if (ipts.size() < 2) { - continue; - } - - double f = cv::norm(ipts.at(0) - ipts.at(1)) / M_PI; - - params.mu() = f; - params.mv() = f; - - setParameters(params); - - for (size_t l = 0; l < objectPoints.size(); ++l) { - estimateExtrinsics( - objectPoints.at(l), imagePoints.at(l), rvecs.at(l), tvecs.at(l)); - } - - double reprojErr = reprojectionError( - objectPoints, imagePoints, rvecs, tvecs, cv::noArray()); - - if (reprojErr < minReprojErr) { - minReprojErr = reprojErr; - f0 = f; - } - } - } - delete[] radius; - } - - if (f0 <= 0.0 && minReprojErr >= std::numeric_limits::max()) { - std::cout << "[" << params.cameraName() << "] " - << "# INFO: kannala-Brandt model fails with given data. " - << std::endl; - - return; - } - - params.mu() = f0; - params.mv() = f0; - - setParameters(params); -} - -// subEigen -void EquidistantCamera::estimateIntrinsics2( - const cv::Size &boardSize, - const std::vector > &objectPoints, - const std::vector > &imagePoints) { - Parameters params = getParameters(); - - double u0 = params.imageWidth() / 2.0; - double v0 = params.imageHeight() / 2.0; - - double minReprojErr = std::numeric_limits::max(); - - std::vector rvecs, tvecs; - rvecs.assign(objectPoints.size(), cv::Mat()); - tvecs.assign(objectPoints.size(), cv::Mat()); - - params.k2() = 0.0; - params.k3() = 0.0; - params.k4() = 0.0; - params.k5() = 0.0; - params.u0() = u0; - params.v0() = v0; - // Initialize focal length // C. Hughes, P. Denny, M. Glavin, and E. Jones, // Equidistant Fish-Eye Calibration and Rectification by Vanishing Point @@ -376,7 +280,7 @@ void EquidistantCamera::estimateIntrinsics2( double f0 = 0.0; for (size_t i = 0; i < imagePoints.size(); ++i) { //std::vector center(boardSize.height); - std::vector center(boardSize.height); + std::vector center(boardSize.height, Ctain::Vector2d(2, 1)); int arrayLength = boardSize.height; double *radius = new double[arrayLength]; memset(radius, 0, arrayLength * sizeof(double)); @@ -446,22 +350,7 @@ void EquidistantCamera::estimateIntrinsics2( * \param p image coordinates * \param P coordinates of the projective ray */ -void EquidistantCamera::liftProjective( - const Eigen::Vector2d &p, Eigen::Vector3d &P) const { - // Lift points to normalised plane - Eigen::Vector2d p_u; - p_u << m_inv_K11 * p(0) + m_inv_K13, m_inv_K22 * p(1) + m_inv_K23; - // Obtain a projective ray - double theta, phi; - backprojectSymmetric(p_u, theta, phi); - - P(0) = sin(theta) * cos(phi); - P(1) = sin(theta) * sin(phi); - P(2) = cos(theta); -} - -// subEigen void EquidistantCamera::liftProjective( const Ctain::Vector2d &p, Ctain::Vector3d &P) const { // Lift points to normalised plane @@ -483,28 +372,7 @@ void EquidistantCamera::liftProjective( * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ -void EquidistantCamera::spaceToPlane( - const Eigen::Vector3d &P, Eigen::Vector2d &p) const { -// double theta = acos(0.5); -// double theta = 0.5; -// double phi = 0.5; -// Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), -// mParameters.k5(), theta) * -// Eigen::Vector2d(cos(0.5), sin(0.5)); - double theta = acos(P(2) / P.norm()); - double phi = atan2(P(1), P(0)); -// double phi = ApproxAtan2(P(1), P(0)); - Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), - mParameters.k5(), theta) * - Eigen::Vector2d(cos(phi), sin(phi)); - - // Apply generalised projection matrix - p << mParameters.mu() * p_u(0) + mParameters.u0(), - mParameters.mv() * p_u(1) + mParameters.v0(); -} - -// subEigen void EquidistantCamera::spaceToPlane( const Ctain::Vector3d &P, Ctain::Vector2d &p) const { // double theta = acos(0.5); @@ -535,22 +403,6 @@ void EquidistantCamera::spaceToPlane( * \param P 3D point coordinates * \param p return value, contains the image point coordinates */ -void EquidistantCamera::spaceToPlane( - const Eigen::Vector3d &P, Eigen::Vector2d &p, - Eigen::Matrix &J) const { - double theta = acos(P(2) / P.norm()); - double phi = atan2(P(1), P(0)); - - Eigen::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), - mParameters.k5(), theta) * - Eigen::Vector2d(cos(phi), sin(phi)); - - // Apply generalised projection matrix - p << mParameters.mu() * p_u(0) + mParameters.u0(), - mParameters.mv() * p_u(1) + mParameters.v0(); -} - -// subEigen void EquidistantCamera::spaceToPlane( const Ctain::Vector3d &P, Ctain::Vector2d &p, Ctain::Matrix23d &J) const { @@ -565,11 +417,10 @@ void EquidistantCamera::spaceToPlane( p << mParameters.mu() * p_u(0) + mParameters.u0() << mParameters.mv() * p_u(1) + mParameters.v0(); } - + void EquidistantCamera::initUndistortMap( cv::Mat &map1, cv::Mat &map2, double fScale) const { cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); - cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); @@ -579,37 +430,9 @@ void EquidistantCamera::initUndistortMap( double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; double theta, phi; - backprojectSymmetric(Eigen::Vector2d(mx_u, my_u), theta, phi); - - Eigen::Vector3d P; - P << sin(theta) * cos(phi), sin(theta) * sin(phi), cos(theta); - - Eigen::Vector2d p; - spaceToPlane(P, p); - - mapX.at(v, u) = p(0); - mapY.at(v, u) = p(1); - } - } - - cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); -} - -// subEigen -void EquidistantCamera::initUndistortMap2( - cv::Mat &map1, cv::Mat &map2, double fScale) const { - cv::Size imageSize(mParameters.imageWidth(), mParameters.imageHeight()); - - cv::Mat mapX = cv::Mat::zeros(imageSize, CV_32F); - cv::Mat mapY = cv::Mat::zeros(imageSize, CV_32F); - - for (int v = 0; v < imageSize.height; ++v) { - for (int u = 0; u < imageSize.width; ++u) { - double mx_u = m_inv_K11 / fScale * u + m_inv_K13 / fScale; - double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; - - double theta, phi; - backprojectSymmetric(Eigen::Vector2d(mx_u, my_u), theta, phi); + Ctain::Vector2d tmp(2, 1); + tmp << mx_u << my_u; + backprojectSymmetric(tmp, theta, phi); Ctain::Vector3d P(3, 1); P << sin(theta) * cos(phi) << sin(theta) * sin(phi) << cos(theta); @@ -625,86 +448,44 @@ void EquidistantCamera::initUndistortMap2( cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); } + cv::Mat EquidistantCamera::initUndistortRectifyMap( cv::Mat &map1, cv::Mat &map2, float fx, float fy, cv::Size imageSize, float cx, float cy, cv::Mat rmat) const { + std::cout <<"w0"; if (imageSize == cv::Size(0, 0)) { imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); } - - cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); - cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); - - Eigen::Matrix3f K_rect; - - if (cx == -1.0f && cy == -1.0f) { - K_rect << fx, 0, imageSize.width / 2, 0, fy, imageSize.height / 2, 0, 0, 1; - } else { - K_rect << fx, 0, cx, 0, fy, cy, 0, 0, 1; - } - - if (fx == -1.0f || fy == -1.0f) { - K_rect(0, 0) = mParameters.mu(); - K_rect(1, 1) = mParameters.mv(); - } - - Eigen::Matrix3f K_rect_inv = K_rect.inverse(); - - Eigen::Matrix3f R, R_inv; - cv::cv2eigen(rmat, R); - R_inv = R.inverse(); - - for (int v = 0; v < imageSize.height; ++v) { - for (int u = 0; u < imageSize.width; ++u) { - Eigen::Vector3f xo; - xo << u, v, 1; - - Eigen::Vector3f uo = R_inv * K_rect_inv * xo; - - Eigen::Vector2d p; - spaceToPlane(uo.cast(), p); - - mapX.at(v, u) = p(0); - mapY.at(v, u) = p(1); - } - } - - cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); - - cv::Mat K_rect_cv; - cv::eigen2cv(K_rect, K_rect_cv); - return K_rect_cv; -} - - -// subEigen -cv::Mat EquidistantCamera::initUndistortRectifyMap2( - cv::Mat &map1, cv::Mat &map2, float fx, float fy, cv::Size imageSize, - float cx, float cy, cv::Mat rmat) const { - if (imageSize == cv::Size(0, 0)) { - imageSize = cv::Size(mParameters.imageWidth(), mParameters.imageHeight()); - } - + + std::cout << std::endl<<"w1"; + cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); cv::Mat mapY = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); Ctain::Matrix3f K_rect(3); + std::cout << std::endl <<"w2"; + if (cx == -1.0f && cy == -1.0f) { K_rect << fx << 0 << imageSize.width / 2 << 0 << fy << imageSize.height / 2 << 0 << 0 << 1; } else { K_rect << fx << 0 << cx << 0 << fy << cy << 0 << 0 << 1; } + std::cout <(i, j); @@ -712,14 +493,15 @@ cv::Mat EquidistantCamera::initUndistortRectifyMap2( } R_inv = R.inverse(); + std::cout<<"R:\n"<< R << std::endl; + std::cout <<"w6"; for (int v = 0; v < imageSize.height; ++v) { for (int u = 0; u < imageSize.width; ++u) { Ctain::Vector3f xo(3, 1); xo << u << v << 1; Ctain::Vector3f uo = R_inv * K_rect_inv * xo; - - Ctain::Vector2d p; + Ctain::Vector2d p(2, 1); spaceToPlane(uo.cast(), p); mapX.at(v, u) = p(0); @@ -727,14 +509,17 @@ cv::Mat EquidistantCamera::initUndistortRectifyMap2( } } + std::cout <<"w7"; cv::convertMaps(mapX, mapY, map1, map2, CV_32FC1, false); - + //std::cout <<"mapX:\n"<(i, j) = K_rect(i, j); } } + + std::cout <<"K_rect_cv:\n"< &x, const std::vector &y, int n, - std::vector &coeffs) const { - std::vector pows; - for (int i = 1; i <= n; i += 2) { - pows.push_back(i); - } - Ctain::MatrixXd X(x.size(), pows.size()); Ctain::MatrixXd Y(y.size(), 1); for (size_t i = 0; i < x.size(); ++i) { @@ -851,89 +610,7 @@ void EquidistantCamera::fitOddPoly2( } void EquidistantCamera::backprojectSymmetric( - const Eigen::Vector2d &p_u, double &theta, double &phi) const { - double tol = 1e-10; - double p_u_norm = p_u.norm(); - - if (p_u_norm < 1e-10) { - phi = 0.0; - } else { - phi = atan2(p_u(1), p_u(0)); - } - - int npow = 9; - if (mParameters.k5() == 0.0) { - npow -= 2; - } - if (mParameters.k4() == 0.0) { - npow -= 2; - } - if (mParameters.k3() == 0.0) { - npow -= 2; - } - if (mParameters.k2() == 0.0) { - npow -= 2; - } - - Eigen::MatrixXd coeffs(npow + 1, 1); - coeffs.setZero(); - coeffs(0) = -p_u_norm; - coeffs(1) = 1.0; - - if (npow >= 3) { - coeffs(3) = mParameters.k2(); - } - if (npow >= 5) { - coeffs(5) = mParameters.k3(); - } - if (npow >= 7) { - coeffs(7) = mParameters.k4(); - } - if (npow >= 9) { - coeffs(9) = mParameters.k5(); - } - - if (npow == 1) { - theta = p_u_norm; - } else { - // Get eigenvalues of companion matrix corresponding to polynomial. - // Eigenvalues correspond to roots of polynomial. - Eigen::MatrixXd A(npow, npow); - A.setZero(); - A.block(1, 0, npow - 1, npow - 1).setIdentity(); - A.col(npow - 1) = -coeffs.block(0, 0, npow, 1) / coeffs(npow); - - Eigen::EigenSolver es(A); - Eigen::MatrixXcd eigval = es.eigenvalues(); - - std::vector thetas; - for (int i = 0; i < eigval.rows(); ++i) { - if (fabs(eigval(i).imag()) > tol) { - continue; - } - - double t = eigval(i).real(); - - if (t < -tol) { - continue; - } else if (t < 0.0) { - t = 0.0; - } - - thetas.push_back(t); - } - - if (thetas.empty()) { - theta = p_u_norm; - } else { - theta = *std::min_element(thetas.begin(), thetas.end()); - } - } -} - -// subEigen - void EquidistantCamera::backprojectSymmetric( - const Ctain::Vector2d &p_u, double &theta, double &phi) const { + const Ctain::Vector2d &p_u, double &theta, double &phi) const { double tol = 1e-10; double p_u_norm = p_u.norm(); @@ -1005,11 +682,11 @@ void EquidistantCamera::backprojectSymmetric( thetas.push_back(t); } - if (thetas.empty()) { - theta = p_u_norm; - } else { - theta = *std::min_element(thetas.begin(), thetas.end()); - } + if (thetas.empty()) { + theta = p_u_norm; + } else { + theta = *std::min_element(thetas.begin(), thetas.end()); + } } } diff --git a/src/mynteye/api/camera_models/equidistant_camera.h b/src/mynteye/api/camera_models/equidistant_camera.h index 6964186..4e8db19 100644 --- a/src/mynteye/api/camera_models/equidistant_camera.h +++ b/src/mynteye/api/camera_models/equidistant_camera.h @@ -115,53 +115,23 @@ class EquidistantCamera : public Camera { const std::vector > &objectPoints, const std::vector > &imagePoints); -//subEigen - void estimateIntrinsics2( - const cv::Size &boardSize, - const std::vector > &objectPoints, - const std::vector > &imagePoints); - - // Lift points from the image plane to the projective space - void liftProjective(const Eigen::Vector2d &p, Eigen::Vector3d &P) const; // NOLINT - // %output P - - //subEigen // Lift points from the image plane to the projective space void liftProjective(const Ctain::Vector2d &p, Ctain::Vector3d &P) const; // NOLINT // %output P - // Projects 3D points to the image plane (Pi function) - void spaceToPlane(const Eigen::Vector3d &P, Eigen::Vector2d &p) const; // NOLINT - // %output p - - //subEigen // Projects 3D points to the image plane (Pi function) void spaceToPlane(const Ctain::Vector3d &P, Ctain::Vector2d &p) const; // NOLINT // %output p + // Projects 3D points to the image plane (Pi function) // and calculates jacobian void spaceToPlane( - const Eigen::Vector3d &P, Eigen::Vector2d &p, // NOLINT - Eigen::Matrix &J) const; // NOLINT + const Ctain::Vector3d &P, Ctain::Vector2d &p, + Ctain::Matrix23d &J) const; // %output p // %output J - // subEigen - // Projects 3D points to the image plane (Pi function) - // and calculates jacobian -void spaceToPlane( - const Ctain::Vector3d &P, Ctain::Vector2d &p, - Ctain::Matrix23d &J) const; // NOLINT - // %output p - // %output J - - template - static void spaceToPlane( - const T *const params, const T *const q, const T *const t, - const Eigen::Matrix &P, Eigen::Matrix &p); // NOLINT - -// subEigen template static void spaceToPlane( const T *const params, const T *const q, const T *const t, @@ -175,15 +145,6 @@ void spaceToPlane( cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f, cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; -// subEigen - void initUndistortMap2( - cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const; // NOLINT - - cv::Mat initUndistortRectifyMap2( - cv::Mat &map1, cv::Mat &map2, float fx = -1.0f, float fy = -1.0f, // NOLINT - cv::Size imageSize = cv::Size(0, 0), float cx = -1.0f, float cy = -1.0f, - cv::Mat rmat = cv::Mat::eye(3, 3, CV_32F)) const; - int parameterCount(void) const; const Parameters &getParameters(void) const; @@ -199,18 +160,9 @@ void spaceToPlane( static T r(T k2, T k3, T k4, T k5, T theta); void fitOddPoly( - const std::vector &x, const std::vector &y, int n, - std::vector &coeffs) const; // NOLINT - -// subEigen - void fitOddPoly2( const std::vector &x, const std::vector &y, int n, std::vector &coeffs) const; - void backprojectSymmetric( - const Eigen::Vector2d &p_u, double &theta, double &phi) const; // NOLINT - -// subEigen void backprojectSymmetric( const Ctain::Vector2d &p_u, double &theta, double &phi) const; // NOLINT @@ -238,48 +190,6 @@ T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) { // theta; } -template -void EquidistantCamera::spaceToPlane( - const T *const params, const T *const q, const T *const t, - const Eigen::Matrix &P, Eigen::Matrix &p) { - T P_w[3]; - P_w[0] = T(P(0)); - P_w[1] = T(P(1)); - P_w[2] = T(P(2)); - - // Convert quaternion from Eigen convention (x, y, z, w) - // to Ceres convention (w, x, y, z) - T q_ceres[4] = {q[3], q[0], q[1], q[2]}; - - T P_c[3]; - QuaternionRotatePoint(q_ceres, P_w, P_c); - - P_c[0] += t[0]; - P_c[1] += t[1]; - P_c[2] += t[2]; - - // project 3D object point to the image plane; - T k2 = params[0]; - T k3 = params[1]; - T k4 = params[2]; - T k5 = params[3]; - T mu = params[4]; - T mv = params[5]; - T u0 = params[6]; - T v0 = params[7]; - - T len = sqrt(P_c[0] * P_c[0] + P_c[1] * P_c[1] + P_c[2] * P_c[2]); - T theta = acos(P_c[2] / len); - T phi = atan2(P_c[1], P_c[0]); - - Eigen::Matrix p_u = - r(k2, k3, k4, k5, theta) * Eigen::Matrix(cos(phi), sin(phi)); - - p(0) = mu * p_u(0) + u0; - p(1) = mv * p_u(1) + v0; -} - -// subEigen template void spaceToPlane( const T *const params, const T *const q, const T *const t, diff --git a/src/mynteye/api/processor/rectify_processor.cc b/src/mynteye/api/processor/rectify_processor.cc index 59607f6..520cd46 100644 --- a/src/mynteye/api/processor/rectify_processor.cc +++ b/src/mynteye/api/processor/rectify_processor.cc @@ -242,6 +242,7 @@ Ctain::Matrix4d RectifyProcessor::loadT(const mynteye::Extrinsics &in) { Ctain::Quaterniond q(R); q.normalize(); Ctain::Matrix4d T(4); + T(3, 3) = 1; T.topLeftCorner<3, 3>() = q.toRotationMatrix(); Ctain::Vector3d t(3, 1); t << t_x << t_y << t_z; @@ -290,6 +291,7 @@ std::shared_ptr RectifyProcessor::stereoRectify( Ctain::Matrix4d T = loadT(ex_right_to_left); Ctain::Matrix3d R = T.topLeftCorner<3, 3>(); Ctain::Vector3d t = T.topRightCorner<3, 1>(); + std::cout<<"T:"< RectifyProcessor::stereoRectify( image_size1, &c_R, &c_t, &c_R1, &c_R2, &c_P1, &c_P2, &T_mul_f, &cx1_min_cx2); - VLOG(2) << "K1: " << K1 << std::endl; - VLOG(2) << "D1: " << D1 << std::endl; - VLOG(2) << "K2: " << K2 << std::endl; - VLOG(2) << "D2: " << D2 << std::endl; - VLOG(2) << "R: " << cv_R << std::endl; - VLOG(2) << "t: " << cv_t << std::endl; - VLOG(2) << "R1: " << R1 << std::endl; - VLOG(2) << "R2: " << R2 << std::endl; - VLOG(2) << "P1: " << P1 << std::endl; - VLOG(2) << "P2: " << P2 << std::endl; + std::cout << "K1: " << K1 << std::endl; + std::cout << "D1: " << D1 << std::endl; + std::cout << "K2: " << K2 << std::endl; + std::cout << "D2: " << D2 << std::endl; + std::cout << "R: " << cv_R << std::endl; + std::cout << "t: " << cv_t << std::endl; + std::cout << "R1: " << R1 << std::endl; + std::cout << "R2: " << R2 << std::endl; + std::cout << "P1: " << P1 << std::endl; + std::cout << "P2: " << P2 << std::endl; R1 = rectifyrad(R1); R2 = rectifyrad(R2); @@ -397,21 +399,31 @@ void RectifyProcessor::InitParams( IntrinsicsEquidistant in_left, IntrinsicsEquidistant in_right, Extrinsics ex_right_to_left) { + LOG(INFO) <<"q1"; calib_model = CalibrationModel::KANNALA_BRANDT; in_left.ResizeIntrinsics(); in_right.ResizeIntrinsics(); in_left_cur = in_left; in_right_cur = in_right; ex_right_to_left_cur = ex_right_to_left; + + LOG(INFO) <<"q2"; + models::CameraPtr camera_odo_ptr_left = generateCameraFromIntrinsicsEquidistant(in_left); models::CameraPtr camera_odo_ptr_right = generateCameraFromIntrinsicsEquidistant(in_right); + + LOG(INFO) <<"q3"; + auto calib_info_tmp = stereoRectify(camera_odo_ptr_left, camera_odo_ptr_right, in_left, in_right, ex_right_to_left); + + LOG(INFO) <<"q4"; + *calib_infos = *calib_info_tmp; cv::Mat rect_R_l = cv::Mat::eye(3, 3, CV_32F), rect_R_r = cv::Mat::eye(3, 3, CV_32F); @@ -421,6 +433,9 @@ void RectifyProcessor::InitParams( rect_R_r.at(i, j) = calib_infos->right.R[i*3+j]; } } + + LOG(INFO) <<"q5"; + double left_f[] = {calib_infos->left.P[0], calib_infos->left.P[5]}; double left_center[] = @@ -429,6 +444,11 @@ void RectifyProcessor::InitParams( {calib_infos->right.P[0], calib_infos->right.P[5]}; double right_center[] = {calib_infos->right.P[2], calib_infos->right.P[6]}; + + LOG(INFO) <<"q6"; + std::cout << "rectR:" << rect_R_l; + LOG(INFO) <<"q9\n"; + std::cout << "left_center:"<initUndistortRectifyMap( map11, map12, left_f[0], left_f[1], cv::Size(0, 0), left_center[0], @@ -437,6 +457,18 @@ void RectifyProcessor::InitParams( map21, map22, right_f[0], right_f[1], cv::Size(0, 0), right_center[0], right_center[1], rect_R_r); +// cv::Mat a; +// cv::Mat b; + + // camera_odo_ptr_left->initUndistortRectifyMap( + // rect_R_l, rect_R_l,0, 0); + // camera_odo_ptr_right->initUndistortRectifyMap( + // map21, map22, right_f[0], right_f[1], + // cv::Size(0, 0), right_center[0], + // right_center[1], rect_R_r); + + LOG(INFO) <<"q7"; + } const char RectifyProcessor::NAME[] = "RectifyProcessor"; @@ -449,11 +481,20 @@ RectifyProcessor::RectifyProcessor( : Processor(std::move(proc_period)), calib_model(CalibrationModel::UNKNOW), _alpha(-1) { + + LOG(INFO) <<"Q"; + calib_infos = std::make_shared(); + + LOG(INFO) <<"W"; + InitParams( *std::dynamic_pointer_cast(intr_left), *std::dynamic_pointer_cast(intr_right), *extr); + + LOG(INFO) <<"E"; + } RectifyProcessor::~RectifyProcessor() { diff --git a/src/mynteye/api/processor/rectify_processor.h b/src/mynteye/api/processor/rectify_processor.h index d15b4a6..c6150fa 100644 --- a/src/mynteye/api/processor/rectify_processor.h +++ b/src/mynteye/api/processor/rectify_processor.h @@ -27,7 +27,6 @@ #include "equidistant_camera.h" #include -#include #include #include