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

refactor(*): delete folder-3rdparty;style(*): change coding styles

Browse Source
This commit is contained in:
Messier
2019-09-04 17:49:30 +08:00
parent 7747a0c3fc
commit 050e7d6771
294 changed files with 61 additions and 80752 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -171,8 +171,9 @@ double Camera::reprojectionError(
}
double Camera::reprojectionError(
const Ctain::Vector3d &P, const Ctain::Quaterniond &camera_q,
const Ctain::Vector3d &camera_t, const Ctain::Vector2d &observed_p) const {
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;

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

@@ -127,7 +127,7 @@ class Camera {
void projectPoints(
const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,
const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const;
const cv::Mat &tvec, std::vector<cv::Point2f> &imagePoints) const;
protected:
cv::Mat m_mask;
};

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

@@ -279,8 +279,9 @@ void EquidistantCamera::estimateIntrinsics(
// f = ||v1 - v2|| / PI;
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, Ctain::Vector2d(2, 1));
// std::vector<Eigen::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));
@@ -330,7 +331,8 @@ void EquidistantCamera::estimateIntrinsics(
delete[] radius;
}
if (f0 <= 0.0 && minReprojErr >= std::numeric_limits<double>::max()) {
if (f0 <= 0.0 &&
minReprojErr >= std::numeric_limits<double>::max()) {
std::cout << "[" << params.cameraName() << "] "
<< "# INFO: kannala-Brandt model fails with given data. "
<< std::endl;
@@ -456,45 +458,34 @@ cv::Mat EquidistantCamera::initUndistortRectifyMap(
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;
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) {
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
R(i, j) = rmat.at<float>(i, j);
}
}
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);
@@ -509,17 +500,14 @@ cv::Mat EquidistantCamera::initUndistortRectifyMap(
}
}
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) {
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;
}
@@ -672,11 +660,11 @@ void EquidistantCamera::backprojectSymmetric(
std::cout << std::endl <<"eigval:" << eigval;
std::vector<double> thetas;
for (int i = 0; i < eigval.rows(); ++i) {
if (fabs(eigval(i, 1)) > tol) { //imag
if (fabs(eigval(i, 1)) > tol) { // imag
continue;
}
double t = eigval(i, 0); //real
double t = eigval(i, 0); // real
if (t < -tol) {
continue;
@@ -689,18 +677,10 @@ void EquidistantCamera::backprojectSymmetric(
if (thetas.empty()) {
theta = p_u_norm;
std::cout<<std::endl<<"empty";
} else {
theta = *std::min_element(thetas.begin(), thetas.end());
// theta = 1.3457661;
std::cout<<"thetas[]:";
for(auto t:thetas)
std::cout<<t<<" ";
}
}
}
std::cout << std::endl <<"thetas:" << theta <<" phi:"<<phi;
}
} // namespace models

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

@@ -161,7 +161,7 @@ class EquidistantCamera : public Camera {
void fitOddPoly(
const std::vector<double> &x, const std::vector<double> &y, int n,
std::vector<double> &coeffs) const;
std::vector<double> &coeffs) const;
void backprojectSymmetric(
const Ctain::Vector2d &p_u, double &theta, double &phi) const; // NOLINT
@@ -192,8 +192,8 @@ T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) {
template <typename T>
void spaceToPlane(
const T *const params, const T *const q, const T *const t,
const Ctain::Matrix<T> &P, Ctain::Matrix<T> &p) {
const T *const params, const T *const q, const T *const t,
const Ctain::Matrix<T> &P, Ctain::Matrix<T> &p) {
T P_w[3];
P_w[0] = T(P(0));
P_w[1] = T(P(1));

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

@@ -177,7 +177,8 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
_alpha = MIN(alpha, 1.);
{
newImgSize = newImgSize.width*newImgSize.height != 0 ? newImgSize : imageSize;
newImgSize = newImgSize.width*newImgSize.height != 0 ?
newImgSize : imageSize;
double cx1_0 = cc_new[0].x;
double cy1_0 = cc_new[0].y;
double cx2_0 = cc_new[1].x;
@@ -291,19 +292,18 @@ 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);
for(int i = 0; i < 3; ++i) {
for(int j = 0; j < 3; ++j) {
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
cv_R.at<double>(i, j) = R(i, j);
}
}
// cv::eigen2cv(t, cv_t);
cv::Mat cv_t(3, 1, CV_64FC1);
for(int i = 0; i < 3; ++i) {
cv_t.at<double>(i, 0) = t(i, 0);
for (int i = 0; i < 3; ++i) {
cv_t.at<double>(i, 0) = t(i, 0);
}
cv::Mat K1, D1, K2, D2;
cv::Size image_size1, image_size2;
@@ -406,24 +406,18 @@ void RectifyProcessor::InitParams(
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);
@@ -433,9 +427,7 @@ 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[] =
@@ -444,11 +436,7 @@ 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],
@@ -456,19 +444,7 @@ void RectifyProcessor::InitParams(
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);
// 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";
right_center[1], rect_R_r);
}
const char RectifyProcessor::NAME[] = "RectifyProcessor";
@@ -481,20 +457,12 @@ 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() {