tami/MYNT-EYE-S-SDK
6
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix(*):fix Matrix-norm() & remove Eigen

Browse Source
This commit is contained in:
Messier 2019-09-04 14:01:28 +08:00
parent 08b3ccb401
commit d0fbd5b1cd
7 changed files with 103 additions and 600 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/Ctain/Matrix.h
View File

@ -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);

109
src/mynteye/api/camera_models/camera.cc
View File

@ -97,46 +97,23 @@ const cv::Mat &Camera::mask(void) const {
return m_mask;
}
void Camera::estimateExtrinsics(
const std::vector<cv::Point3f> &objectPoints,
const std::vector<cv::Point2f> &imagePoints, cv::Mat &rvec,
cv::Mat &tvec) const {
std::vector<cv::Point2f> Ms(imagePoints.size());
for (size_t i = 0; i < Ms.size(); ++i) {
Eigen::Vector3d P;
// 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);
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
cv::solvePnP(
objectPoints, Ms, cv::Mat::eye(3, 3, CV_64F), cv::noArray(), rvec, tvec);
}
//subEigen
void Camera::estimateExtrinsics2(
const std::vector<cv::Point3f> &objectPoints,
const std::vector<cv::Point2f> &imagePoints, cv::Mat &rvec,
cv::Mat &tvec) const {
std::vector<cv::Point2f> 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<double>(0, 0), R0.at<double>(0, 1), R0.at<double>(0, 2),
R0.at<double>(1, 0), R0.at<double>(1, 1), R0.at<double>(1, 2),
R0.at<double>(2, 0), R0.at<double>(2, 1), R0.at<double>(2, 2);
Eigen::Vector3d t;
t << tvec.at<double>(0), tvec.at<double>(1), tvec.at<double>(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<cv::Point3f> &objectPoints, const cv::Mat &rvec,
const cv::Mat &tvec, std::vector<cv::Point2f> &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<double>(0, 0) << R0.at<double>(0, 1) << R0.at<double>(0, 2) <<
R0.at<double>(1, 0) << R0.at<double>(1, 1) << R0.at<double>(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)));

47
src/mynteye/api/camera_models/camera.h
View File

@ -16,7 +16,6 @@
#include <vector>
#include <memory>
#include "eigen3/Eigen/Dense"
#include "Ctain/CtainBase.h"
#include <opencv2/core/core.hpp>
@ -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<std::vector<cv::Point3f> > &objectPoints,
const std::vector<std::vector<cv::Point2f> > &imagePoints) = 0;
// subEigen
virtual void estimateIntrinsics2(
const cv::Size &boardSize,
const std::vector<std::vector<cv::Point3f> > &objectPoints,
const std::vector<std::vector<cv::Point2f> > &imagePoints) = 0;
virtual void estimateExtrinsics(
const std::vector<cv::Point3f> &objectPoints,
const std::vector<cv::Point2f> &imagePoints,
cv::Mat &rvec, cv::Mat &tvec) const; // NOLINT
virtual void estimateExtrinsics2(
const std::vector<cv::Point3f> &objectPoints,
const std::vector<cv::Point2f> &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<std::vector<cv::Point3f> > &objectPoints,
@ -151,26 +121,13 @@ class Camera {
const std::vector<cv::Mat> &rvecs, const std::vector<cv::Mat> &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<cv::Point3f> &objectPoints, const cv::Mat &rvec,
const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const; // NOLINT
//subEigen
void projectPoints2(
const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,
const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const; // NOLINT
const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const;
protected:
cv::Mat m_mask;
};

377
src/mynteye/api/camera_models/equidistant_camera.cc
View File

@ -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<Eigen::Vector2d> 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<cv::Point2d> 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<cv::Point2d> 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<double>::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<std::vector<cv::Point3f> > &objectPoints,
const std::vector<std::vector<cv::Point2f> > &imagePoints) {
Parameters params = getParameters();
double u0 = params.imageWidth() / 2.0;
double v0 = params.imageHeight() / 2.0;
double minReprojErr = std::numeric_limits<double>::max();
std::vector<cv::Mat> 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<Eigen::Vector2d> center(boardSize.height);
std::vector<Ctain::Vector2d> center(boardSize.height);
std::vector<Ctain::Vector2d> 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<double, 2, 3> &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 {
@ -569,7 +421,6 @@ void EquidistantCamera::spaceToPlane(
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<float>(v, u) = p(0);
mapY.at<float>(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<double>(), p);
mapX.at<float>(v, u) = p(0);
mapY.at<float>(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 <<std::endl<<"w3";
if (fx == -1.0f || fy == -1.0f) {
K_rect(0, 0) = mParameters.mu();
K_rect(1, 1) = mParameters.mv();
}
std::cout <<std::endl<<"w4";
Ctain::Matrix3f K_rect_inv = K_rect.inverse();
Ctain::Matrix3f R(3), R_inv(3);
std::cout <<std::endl<<"w5";
for(int i = 0; i < 3; ++i) {
for(int j = 0; j < 3; ++j) {
R(i, j) = rmat.at<float>(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<double>(), p);
mapX.at<float>(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"<<mapY;
cv::Mat K_rect_cv(3, 3, CV_32FC1);
for(int i = 0; i < 3; ++i) {
for(int j = 0; j < 3; ++j) {
K_rect_cv.at<float>(i, j) = K_rect(i, j);
}
}
std::cout <<"K_rect_cv:\n"<<K_rect_cv;
return K_rect_cv;
}
@ -806,32 +591,6 @@ void EquidistantCamera::fitOddPoly(
pows.push_back(i);
}
Eigen::MatrixXd X(x.size(), pows.size());
Eigen::MatrixXd Y(y.size(), 1);
for (size_t i = 0; i < x.size(); ++i) {
for (size_t j = 0; j < pows.size(); ++j) {
X(i, j) = pow(x.at(i), pows.at(j));
}
Y(i, 0) = y.at(i);
}
Eigen::MatrixXd A = (X.transpose() * X).inverse() * X.transpose() * Y;
coeffs.resize(A.rows());
for (int i = 0; i < A.rows(); ++i) {
coeffs.at(i) = A(i, 0);
}
}
// subEigen
void EquidistantCamera::fitOddPoly2(
const std::vector<double> &x, const std::vector<double> &y, int n,
std::vector<double> &coeffs) const {
std::vector<int> 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<Eigen::MatrixXd> es(A);
Eigen::MatrixXcd eigval = es.eigenvalues();
std::vector<double> 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());
}
}
}

96
src/mynteye/api/camera_models/equidistant_camera.h
View File

@ -115,53 +115,23 @@ class EquidistantCamera : public Camera {
const std::vector<std::vector<cv::Point3f> > &objectPoints,
const std::vector<std::vector<cv::Point2f> > &imagePoints);
//subEigen
void estimateIntrinsics2(
const cv::Size &boardSize,
const std::vector<std::vector<cv::Point3f> > &objectPoints,
const std::vector<std::vector<cv::Point2f> > &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<double, 2, 3> &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 <typename T>
static void spaceToPlane(
const T *const params, const T *const q, const T *const t,
const Eigen::Matrix<T, 3, 1> &P, Eigen::Matrix<T, 2, 1> &p); // NOLINT
// subEigen
template <typename T>
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<double> &x, const std::vector<double> &y, int n,
std::vector<double> &coeffs) const; // NOLINT
// subEigen
void fitOddPoly2(
const std::vector<double> &x, const std::vector<double> &y, int n,
std::vector<double> &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 <typename T>
void EquidistantCamera::spaceToPlane(
const T *const params, const T *const q, const T *const t,
const Eigen::Matrix<T, 3, 1> &P, Eigen::Matrix<T, 2, 1> &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<T, 2, 1> p_u =
r(k2, k3, k4, k5, theta) * Eigen::Matrix<T, 2, 1>(cos(phi), sin(phi));
p(0) = mu * p_u(0) + u0;
p(1) = mv * p_u(1) + v0;
}
// subEigen
template <typename T>
void spaceToPlane(
const T *const params, const T *const q, const T *const t,

61
src/mynteye/api/processor/rectify_processor.cc
View File

@ -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<struct CameraROSMsgInfoPair> 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:"<<T;
// cv::Mat cv_R, cv_t;
// cv::eigen2cv(R, cv_R);
cv::Mat cv_R(3, 3, CV_64FC1);
@ -326,16 +328,16 @@ std::shared_ptr<struct CameraROSMsgInfoPair> 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<float>(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:"<<left_center[0]<<","<<left_center[1]<<std::endl;
camera_odo_ptr_left->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<struct CameraROSMsgInfoPair>();
LOG(INFO) <<"W";
InitParams(
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_left),
*std::dynamic_pointer_cast<IntrinsicsEquidistant>(intr_right),
*extr);
LOG(INFO) <<"E";
}
RectifyProcessor::~RectifyProcessor() {

1
src/mynteye/api/processor/rectify_processor.h
View File

@ -27,7 +27,6 @@
#include "equidistant_camera.h"
#include <opencv2/core/core.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <opencv2/highgui/highgui.hpp>