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

refactor(*): change namespace Ctain to mynteye::ctain

Browse Source
This commit is contained in:
Messier 2019-09-05 15:09:22 +08:00
parent fff6c84032
commit 72de43877c
11 changed files with 120 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
src/mynteye/api/camera_models/CtainBase.h
View File

@ -16,11 +16,14 @@
#include "SquareMatrix.h" #include "SquareMatrix.h"
#include "MatrixSolver.h" #include "MatrixSolver.h"
#include "Quaternion.h" #include "Quaternion.h"
#ifndef MATRIX_CTAIN_H #ifndef SRC_MYNTEYE_API_CAMERA_MODELS_CTAINBASE_H_
#define MATRIX_CTAIN_H #define SRC_MYNTEYE_API_CAMERA_MODELS_CTAINBASE_H_
#include "mynteye/mynteye.h"
namespace Ctain { MYNTEYE_BEGIN_NAMESPACE
namespace ctain {
typedef SMatrix<double> Matrixd; typedef SMatrix<double> Matrixd;
typedef Matrix<double> MatrixXd; typedef Matrix<double> MatrixXd;
typedef Matrix<double> Matrix23d; typedef Matrix<double> Matrix23d;
@ -43,4 +46,5 @@ namespace Ctain {
typedef Quaternion<double> Quaterniond; typedef Quaternion<double> Quaterniond;
} }
#endif // Ctain_CtainBASE_H MYNTEYE_END_NAMESPACE
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_CTAINBASE_H_

14
src/mynteye/api/camera_models/Matrix.h
View File

@ -19,7 +19,11 @@
#include <iostream> #include <iostream>
#include <cmath> #include <cmath>
namespace Ctain { #include "mynteye/mynteye.h"
MYNTEYE_BEGIN_NAMESPACE
namespace ctain {
template<typename _Scalar> template<typename _Scalar>
class Matrix { class Matrix {
public: public:
@ -280,7 +284,6 @@ class Matrix {
inline _Scalar cData(int i, int j) const { inline _Scalar cData(int i, int j) const {
return data[id(i, j)]; return data[id(i, j)];
} }
}; // class Matrix }; // class Matrix
template<typename _Scalar> template<typename _Scalar>
@ -346,8 +349,7 @@ Matrix<_Scalar> Matrix<_Scalar>::operator -(const Matrix<_Scalar> &m) const {
} }
template<typename _Scalar> template<typename _Scalar>
Matrix<_Scalar> Matrix<_Scalar>::transpose() const Matrix<_Scalar> Matrix<_Scalar>::transpose() const {
{
Matrix<_Scalar> res(_Cols, _Rows); Matrix<_Scalar> res(_Cols, _Rows);
for (int i = 0; i < _Rows; i++) { for (int i = 0; i < _Rows; i++) {
for (int j = 0; j < _Cols; j++) { for (int j = 0; j < _Cols; j++) {
@ -417,5 +419,7 @@ double Matrix<_Scalar>::norm() const {
return sum; return sum;
} }
} // namespace Ctain } // namespace ctain
MYNTEYE_END_NAMESPACE
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_MATRIX_H_ #endif // SRC_MYNTEYE_API_CAMERA_MODELS_MATRIX_H_

21
src/mynteye/api/camera_models/MatrixSolver.h
View File

@ -1,17 +1,31 @@
// Copyright 2018 Slightech Co., Ltd. All rights reserved.
// //
// Created by 顾涵彬 on 2019-08-30. // Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
// //
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_ #ifndef SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_
#define SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_ #define SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_
#include <cmath> #include <cmath>
#include <complex> #include <complex>
#include "mynteye/mynteye.h"
MYNTEYE_BEGIN_NAMESPACE
static bool Matrix_EigenValue(double *K1, int n, static bool Matrix_EigenValue(double *K1, int n,
int LoopNumber, double Error1, double *Ret); int LoopNumber, double Error1, double *Ret);
static void Matrix_Hessenberg(double *A1, int n, double *ret); static void Matrix_Hessenberg(double *A1, int n, double *ret);
namespace Ctain { namespace ctain {
class EigenSolver { class EigenSolver {
public: public:
explicit EigenSolver(SMatrix<double> s) { explicit EigenSolver(SMatrix<double> s) {
@ -35,7 +49,7 @@ class EigenSolver {
Matrix<double> t; Matrix<double> t;
}; };
} // namespace Ctain } // namespace ctain
static void Matrix_Hessenberg(double *A1, int n, double *ret) { static void Matrix_Hessenberg(double *A1, int n, double *ret) {
int MaxNumber; int MaxNumber;
@ -239,4 +253,5 @@ static bool Matrix_EigenValue(double *K1, int n,
return true; return true;
} }
MYNTEYE_END_NAMESPACE
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_ #endif // SRC_MYNTEYE_API_CAMERA_MODELS_MATRIXSOLVER_H_

11
src/mynteye/api/camera_models/Quaternion.h
View File

@ -17,7 +17,12 @@
#include "SquareMatrix.h" #include "SquareMatrix.h"
#include <cmath> #include <cmath>
namespace Ctain {
#include "mynteye/mynteye.h"
MYNTEYE_BEGIN_NAMESPACE
namespace ctain {
template<typename T> template<typename T>
class Quaternion { class Quaternion {
public: public:
@ -62,5 +67,7 @@ class Quaternion {
T _z; T _z;
T _w; T _w;
}; };
} // namespace Ctain } // namespace ctain
MYNTEYE_END_NAMESPACE
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_QUATERNION_H_ #endif // SRC_MYNTEYE_API_CAMERA_MODELS_QUATERNION_H_

11
src/mynteye/api/camera_models/SquareMatrix.h
View File

@ -15,7 +15,12 @@
#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_ #ifndef SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_
#define SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_ #define SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_
#include "Matrix.h" #include "Matrix.h"
namespace Ctain {
#include "mynteye/mynteye.h"
MYNTEYE_BEGIN_NAMESPACE
namespace ctain {
#define Matrix_ Matrix<_Scalar> #define Matrix_ Matrix<_Scalar>
template<typename _Scalar> template<typename _Scalar>
class SMatrix: public Matrix_{ class SMatrix: public Matrix_{
@ -92,5 +97,7 @@ _Scalar SMatrix<_Scalar>::M(int m, int n) {
} }
#undef Matrix_ #undef Matrix_
} // namespace Ctain } // namespace ctain
MYNTEYE_END_NAMESPACE
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_ #endif // SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_

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

@ -105,7 +105,7 @@ void Camera::estimateExtrinsics(
std::vector<cv::Point2f> Ms(imagePoints.size()); std::vector<cv::Point2f> Ms(imagePoints.size());
for (size_t i = 0; i < Ms.size(); ++i) { for (size_t i = 0; i < Ms.size(); ++i) {
// Eigen::Vector3d P; // Eigen::Vector3d P;
Ctain::Vectord P(3, 1), p(2, 1); ctain::Vectord P(3, 1), p(2, 1);
p<< imagePoints.at(i).x << imagePoints.at(i).y; p<< imagePoints.at(i).x << imagePoints.at(i).y;
// liftProjective( // liftProjective(
@ -122,8 +122,8 @@ void Camera::estimateExtrinsics(
} }
double Camera::reprojectionDist( double Camera::reprojectionDist(
const Ctain::Vector3d &P1, const Ctain::Vector3d &P2) const { const ctain::Vector3d &P1, const ctain::Vector3d &P2) const {
Ctain::Vector2d p1(2, 1), p2(2, 1); ctain::Vector2d p1(2, 1), p2(2, 1);
spaceToPlane(P1, p1); spaceToPlane(P1, p1);
spaceToPlane(P2, p2); spaceToPlane(P2, p2);
@ -171,13 +171,13 @@ double Camera::reprojectionError(
} }
double Camera::reprojectionError( double Camera::reprojectionError(
const Ctain::Vector3d &P, const Ctain::Quaterniond &camera_q, const ctain::Vector3d &P, const ctain::Quaterniond &camera_q,
const Ctain::Vector3d &camera_t, const ctain::Vector3d &camera_t,
const Ctain::Vector2d &observed_p) const { const ctain::Vector2d &observed_p) const {
Ctain::Vector3d P_cam; ctain::Vector3d P_cam;
P_cam = camera_q.toRotationMatrix() * P + camera_t; P_cam = camera_q.toRotationMatrix() * P + camera_t;
Ctain::Vector2d p(2, 1), res(2, 1); ctain::Vector2d p(2, 1), res(2, 1);
spaceToPlane(P_cam, p); spaceToPlane(P_cam, p);
res = p - observed_p; res = p - observed_p;
return res.norm(); return res.norm();
@ -193,24 +193,24 @@ void Camera::projectPoints(
cv::Mat R0; cv::Mat R0;
cv::Rodrigues(rvec, R0); cv::Rodrigues(rvec, R0);
Ctain::MatrixXd R(3, 3); ctain::MatrixXd R(3, 3);
R << R0.at<double>(0, 0) << R0.at<double>(0, 1) << R0.at<double>(0, 2) << 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>(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); R0.at<double>(2, 0) << R0.at<double>(2, 1) << R0.at<double>(2, 2);
Ctain::Vectord t(3, 1); ctain::Vectord t(3, 1);
t << tvec.at<double>(0) << tvec.at<double>(1) << tvec.at<double>(2); t << tvec.at<double>(0) << tvec.at<double>(1) << tvec.at<double>(2);
for (size_t i = 0; i < objectPoints.size(); ++i) { for (size_t i = 0; i < objectPoints.size(); ++i) {
const cv::Point3f &objectPoint = objectPoints.at(i); const cv::Point3f &objectPoint = objectPoints.at(i);
// Rotate and translate // Rotate and translate
Ctain::Vectord P(3, 1); ctain::Vectord P(3, 1);
P << objectPoint.x << objectPoint.y << objectPoint.z; P << objectPoint.x << objectPoint.y << objectPoint.z;
P = R * P + t; P = R * P + t;
Ctain::Vector2d p(2, 1); ctain::Vector2d p(2, 1);
spaceToPlane(P, p); spaceToPlane(P, p);
imagePoints.push_back(cv::Point2f(p(0), p(1))); imagePoints.push_back(cv::Point2f(p(0), p(1)));

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

@ -76,12 +76,12 @@ class Camera {
// Lift points from the image plane to the projective space // Lift points from the image plane to the projective space
virtual void liftProjective( virtual void liftProjective(
const Ctain::Vector2d &p, Ctain::Vector3d &P) const = 0; // NOLINT const ctain::Vector2d &p, ctain::Vector3d &P) const = 0; // NOLINT
// %output P // %output P
// Projects 3D points to the image plane (Pi function) // Projects 3D points to the image plane (Pi function)
virtual void spaceToPlane( virtual void spaceToPlane(
const Ctain::Vector3d &P, Ctain::Vector2d &p) const = 0; // NOLINT const ctain::Vector3d &P, ctain::Vector2d &p) const = 0; // NOLINT
// %output p // %output p
// Projects 3D points to the image plane (Pi function) // Projects 3D points to the image plane (Pi function)
@ -113,7 +113,7 @@ class Camera {
* \return euclidean distance in the plane * \return euclidean distance in the plane
*/ */
double reprojectionDist( double reprojectionDist(
const Ctain::Vector3d &P1, const Ctain::Vector3d &P2) const; const ctain::Vector3d &P1, const ctain::Vector3d &P2) const;
double reprojectionError( double reprojectionError(
const std::vector<std::vector<cv::Point3f> > &objectPoints, const std::vector<std::vector<cv::Point3f> > &objectPoints,
@ -122,8 +122,8 @@ class Camera {
cv::OutputArray perViewErrors = cv::noArray()) const; cv::OutputArray perViewErrors = cv::noArray()) const;
double reprojectionError( double reprojectionError(
const Ctain::Vector3d &P, const Ctain::Quaterniond &camera_q, const ctain::Vector3d &P, const ctain::Quaterniond &camera_q,
const Ctain::Vector3d &camera_t, const Ctain::Vector2d &observed_p) const; const ctain::Vector3d &camera_t, const ctain::Vector2d &observed_p) const;
void projectPoints( void projectPoints(
const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec, const std::vector<cv::Point3f> &objectPoints, const cv::Mat &rvec,

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

@ -280,8 +280,8 @@ void EquidistantCamera::estimateIntrinsics(
double f0 = 0.0; double f0 = 0.0;
for (size_t i = 0; i < imagePoints.size(); ++i) { for (size_t i = 0; i < imagePoints.size(); ++i) {
// std::vector<Eigen::Vector2d> center(boardSize.height); // std::vector<Eigen::Vector2d> center(boardSize.height);
std::vector<Ctain::Vector2d> center( std::vector<ctain::Vector2d> center(
boardSize.height, Ctain::Vector2d(2, 1)); boardSize.height, ctain::Vector2d(2, 1));
int arrayLength = boardSize.height; int arrayLength = boardSize.height;
double *radius = new double[arrayLength]; double *radius = new double[arrayLength];
memset(radius, 0, arrayLength * sizeof(double)); memset(radius, 0, arrayLength * sizeof(double));
@ -354,9 +354,9 @@ void EquidistantCamera::estimateIntrinsics(
*/ */
void EquidistantCamera::liftProjective( void EquidistantCamera::liftProjective(
const Ctain::Vector2d &p, Ctain::Vector3d &P) const { const ctain::Vector2d &p, ctain::Vector3d &P) const {
// Lift points to normalised plane // Lift points to normalised plane
Ctain::Vector2d p_u(2, 1); ctain::Vector2d p_u(2, 1);
p_u << m_inv_K11 * p(0) + m_inv_K13 << m_inv_K22 * p(1) + m_inv_K23; p_u << m_inv_K11 * p(0) + m_inv_K13 << m_inv_K22 * p(1) + m_inv_K23;
// Obtain a projective ray // Obtain a projective ray
@ -376,7 +376,7 @@ void EquidistantCamera::liftProjective(
*/ */
void EquidistantCamera::spaceToPlane( void EquidistantCamera::spaceToPlane(
const Ctain::Vector3d &P, Ctain::Vector2d &p) const { const ctain::Vector3d &P, ctain::Vector2d &p) const {
// double theta = acos(0.5); // double theta = acos(0.5);
// double theta = 0.5; // double theta = 0.5;
// double phi = 0.5; // double phi = 0.5;
@ -390,9 +390,9 @@ void EquidistantCamera::spaceToPlane(
// double phi = ApproxAtan2(P(1), P(0)); // double phi = ApproxAtan2(P(1), P(0));
double tmp[2] = {cos(phi), sin(phi)}; double tmp[2] = {cos(phi), sin(phi)};
Ctain::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), ctain::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(),
mParameters.k5(), theta) * mParameters.k5(), theta) *
Ctain::Vector2d(tmp, 2, 1); ctain::Vector2d(tmp, 2, 1);
// Apply generalised projection matrix // Apply generalised projection matrix
p << mParameters.mu() * p_u(0) + mParameters.u0() p << mParameters.mu() * p_u(0) + mParameters.u0()
@ -406,14 +406,14 @@ void EquidistantCamera::spaceToPlane(
* \param p return value, contains the image point coordinates * \param p return value, contains the image point coordinates
*/ */
void EquidistantCamera::spaceToPlane( void EquidistantCamera::spaceToPlane(
const Ctain::Vector3d &P, Ctain::Vector2d &p, const ctain::Vector3d &P, ctain::Vector2d &p,
Ctain::Matrix23d &J) const { ctain::Matrix23d &J) const {
double theta = acos(P(2) / 3.0); double theta = acos(P(2) / 3.0);
double phi = atan2(P(1), P(0)); double phi = atan2(P(1), P(0));
double tmp[2] = {cos(phi), sin(phi)}; double tmp[2] = {cos(phi), sin(phi)};
Ctain::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(), ctain::Vector2d p_u = r(mParameters.k2(), mParameters.k3(), mParameters.k4(),
mParameters.k5(), theta) * mParameters.k5(), theta) *
Ctain::Vector2d(tmp, 2, 1); ctain::Vector2d(tmp, 2, 1);
// Apply generalised projection matrix // Apply generalised projection matrix
p << mParameters.mu() * p_u(0) + mParameters.u0() p << mParameters.mu() * p_u(0) + mParameters.u0()
@ -432,14 +432,14 @@ void EquidistantCamera::initUndistortMap(
double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale; double my_u = m_inv_K22 / fScale * v + m_inv_K23 / fScale;
double theta, phi; double theta, phi;
Ctain::Vector2d tmp(2, 1); ctain::Vector2d tmp(2, 1);
tmp << mx_u << my_u; tmp << mx_u << my_u;
backprojectSymmetric(tmp, theta, phi); backprojectSymmetric(tmp, theta, phi);
Ctain::Vector3d P(3, 1); ctain::Vector3d P(3, 1);
P << sin(theta) * cos(phi) << sin(theta) * sin(phi) << cos(theta); P << sin(theta) * cos(phi) << sin(theta) * sin(phi) << cos(theta);
Ctain::Vector2d p(2, 1); ctain::Vector2d p(2, 1);
spaceToPlane(P, p); spaceToPlane(P, p);
mapX.at<float>(v, u) = p(0); mapX.at<float>(v, u) = p(0);
@ -462,7 +462,7 @@ cv::Mat EquidistantCamera::initUndistortRectifyMap(
cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F); cv::Mat mapX = cv::Mat::zeros(imageSize.height, imageSize.width, CV_32F);
cv::Mat mapY = 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); ctain::Matrix3f K_rect(3);
if (cx == -1.0f && cy == -1.0f) { if (cx == -1.0f && cy == -1.0f) {
K_rect << fx << 0 << imageSize.width / 2 << K_rect << fx << 0 << imageSize.width / 2 <<
@ -476,8 +476,8 @@ cv::Mat EquidistantCamera::initUndistortRectifyMap(
K_rect(1, 1) = mParameters.mv(); K_rect(1, 1) = mParameters.mv();
} }
Ctain::Matrix3f K_rect_inv = K_rect.inverse(); ctain::Matrix3f K_rect_inv = K_rect.inverse();
Ctain::Matrix3f R(3), R_inv(3); ctain::Matrix3f R(3), R_inv(3);
for (int i = 0; i < 3; ++i) { for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) { for (int j = 0; j < 3; ++j) {
@ -488,11 +488,11 @@ cv::Mat EquidistantCamera::initUndistortRectifyMap(
for (int v = 0; v < imageSize.height; ++v) { for (int v = 0; v < imageSize.height; ++v) {
for (int u = 0; u < imageSize.width; ++u) { for (int u = 0; u < imageSize.width; ++u) {
Ctain::Vector3f xo(3, 1); ctain::Vector3f xo(3, 1);
xo << u << v << 1; xo << u << v << 1;
Ctain::Vector3f uo = R_inv * K_rect_inv * xo; ctain::Vector3f uo = R_inv * K_rect_inv * xo;
Ctain::Vector2d p(2, 1); ctain::Vector2d p(2, 1);
spaceToPlane(uo.cast<double>(), p); spaceToPlane(uo.cast<double>(), p);
mapX.at<float>(v, u) = p(0); mapX.at<float>(v, u) = p(0);
@ -579,16 +579,16 @@ void EquidistantCamera::fitOddPoly(
pows.push_back(i); pows.push_back(i);
} }
Ctain::MatrixXd X(x.size(), pows.size()); ctain::MatrixXd X(x.size(), pows.size());
Ctain::MatrixXd Y(y.size(), 1); ctain::MatrixXd Y(y.size(), 1);
for (size_t i = 0; i < x.size(); ++i) { for (size_t i = 0; i < x.size(); ++i) {
for (size_t j = 0; j < pows.size(); ++j) { for (size_t j = 0; j < pows.size(); ++j) {
X(i, j) = pow(x.at(i), pows.at(j)); X(i, j) = pow(x.at(i), pows.at(j));
} }
Y(i, 0) = y.at(i); Y(i, 0) = y.at(i);
} }
Ctain::SMatrix<double> Tmp(X.transpose() * X); ctain::SMatrix<double> Tmp(X.transpose() * X);
Ctain::MatrixXd A = Tmp.inverse() * X.transpose() * Y; ctain::MatrixXd A = Tmp.inverse() * X.transpose() * Y;
coeffs.resize(A.rows()); coeffs.resize(A.rows());
for (int i = 0; i < A.rows(); ++i) { for (int i = 0; i < A.rows(); ++i) {
@ -597,7 +597,7 @@ void EquidistantCamera::fitOddPoly(
} }
void EquidistantCamera::backprojectSymmetric( 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 tol = 1e-10;
double p_u_norm = p_u.norm(); double p_u_norm = p_u.norm();
@ -621,7 +621,7 @@ void EquidistantCamera::backprojectSymmetric(
npow -= 2; npow -= 2;
} }
Ctain::MatrixXd coeffs(npow + 1, 1); ctain::MatrixXd coeffs(npow + 1, 1);
coeffs.setZero(); coeffs.setZero();
coeffs(0) = -p_u_norm; coeffs(0) = -p_u_norm;
coeffs(1) = 1.0; coeffs(1) = 1.0;
@ -644,17 +644,17 @@ void EquidistantCamera::backprojectSymmetric(
} else { } else {
// Get eigenvalues of companion matrix corresponding to polynomial. // Get eigenvalues of companion matrix corresponding to polynomial.
// Eigenvalues correspond to roots of polynomial. // Eigenvalues correspond to roots of polynomial.
Ctain::Matrixd A(npow); ctain::Matrixd A(npow);
A.setZero(); A.setZero();
A.block(1, 0, npow - 1, npow - 1).setIdentity(); A.block(1, 0, npow - 1, npow - 1).setIdentity();
A.col(npow - 1) = -coeffs.block(0, 0, npow, 1) / coeffs(npow); A.col(npow - 1) = -coeffs.block(0, 0, npow, 1) / coeffs(npow);
std::cout << std::endl <<"A:" << A; std::cout << std::endl <<"A:" << A;
Ctain::EigenSolver es(A); ctain::EigenSolver es(A);
Ctain::Matrix<double> eigval(9, 2); ctain::Matrix<double> eigval(9, 2);
eigval = es.eigenvalues(); eigval = es.eigenvalues();
// Ctain::EigenSolver es(A); // ctain::EigenSolver es(A);
// Ctain::MatrixXcd eigval(npow, 2); // ctain::MatrixXcd eigval(npow, 2);
// eigval = es.eigenvalues(); // eigval = es.eigenvalues();
std::cout << std::endl <<"eigval:" << eigval; std::cout << std::endl <<"eigval:" << eigval;
std::vector<double> thetas; std::vector<double> thetas;

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

@ -116,26 +116,26 @@ class EquidistantCamera : public Camera {
const std::vector<std::vector<cv::Point2f> > &imagePoints); const std::vector<std::vector<cv::Point2f> > &imagePoints);
// Lift points from the image plane to the projective space // Lift points from the image plane to the projective space
void liftProjective(const Ctain::Vector2d &p, Ctain::Vector3d &P) const; // NOLINT void liftProjective(const ctain::Vector2d &p, ctain::Vector3d &P) const; // NOLINT
// %output P // %output P
// Projects 3D points to the image plane (Pi function) // Projects 3D points to the image plane (Pi function)
void spaceToPlane(const Ctain::Vector3d &P, Ctain::Vector2d &p) const; // NOLINT void spaceToPlane(const ctain::Vector3d &P, ctain::Vector2d &p) const; // NOLINT
// %output p // %output p
// Projects 3D points to the image plane (Pi function) // Projects 3D points to the image plane (Pi function)
// and calculates jacobian // and calculates jacobian
void spaceToPlane( void spaceToPlane(
const Ctain::Vector3d &P,Ctain::Vector2d &p, // NOLINT const ctain::Vector3d &P,ctain::Vector2d &p, // NOLINT
Ctain::Matrix23d &J) const; // NOLINT ctain::Matrix23d &J) const; // NOLINT
// %output p // %output p
// %output J // %output J
template <typename T> template <typename T>
static void spaceToPlane( static void spaceToPlane(
const T *const params, const T *const q, const T *const t, const T *const params, const T *const q, const T *const t,
const Ctain::Matrix<T> &P, Ctain::Matrix<T> &p); // NOLINT const ctain::Matrix<T> &P, ctain::Matrix<T> &p); // NOLINT
void initUndistortMap( void initUndistortMap(
cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const; // NOLINT cv::Mat &map1, cv::Mat &map2, double fScale = 1.0) const; // NOLINT
@ -164,7 +164,7 @@ class EquidistantCamera : public Camera {
std::vector<double> &coeffs) const; // NOLINT std::vector<double> &coeffs) const; // NOLINT
void backprojectSymmetric( void backprojectSymmetric(
const Ctain::Vector2d &p_u, double &theta, double &phi) const; // NOLINT const ctain::Vector2d &p_u, double &theta, double &phi) const; // NOLINT
Parameters mParameters; Parameters mParameters;
@ -193,7 +193,7 @@ T EquidistantCamera::r(T k2, T k3, T k4, T k5, T theta) {
template <typename T> template <typename T>
void spaceToPlane( void spaceToPlane(
const T *const params, const T *const q, const T *const t, const T *const params, const T *const q, const T *const t,
const Ctain::Matrix<T> &P, Ctain::Matrix<T> &p) { // NOLINT const ctain::Matrix<T> &P, ctain::Matrix<T> &p) { // NOLINT
T P_w[3]; T P_w[3];
P_w[0] = T(P(0)); P_w[0] = T(P(0));
P_w[1] = T(P(1)); P_w[1] = T(P(1));
@ -224,7 +224,7 @@ void spaceToPlane(
T theta = acos(P_c[2] / len); T theta = acos(P_c[2] / len);
T phi = atan2(P_c[1], P_c[0]); T phi = atan2(P_c[1], P_c[0]);
Ctain::Matrix<T> p_u(2, 1), tmp(2, 1); ctain::Matrix<T> p_u(2, 1), tmp(2, 1);
tmp(0) = cos(phi); tmp(0) = cos(phi);
tmp(1) = sin(phi); tmp(1) = sin(phi);
p_u = r(k2, k3, k4, k5, theta) * tmp; p_u = r(k2, k3, k4, k5, theta) * tmp;

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

@ -126,8 +126,8 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3); CvMat pts_3 = cvMat(1, 4, CV_32FC3, _pts_3);
// Eigen::Vector2d a; // Eigen::Vector2d a;
// Eigen::Vector3d b; // Eigen::Vector3d b;
Ctain::Vector2d a(2, 1); ctain::Vector2d a(2, 1);
Ctain::Vector3d b(3, 1); ctain::Vector3d b(3, 1);
for (i = 0; i < 4; i++) { for (i = 0; i < 4; i++) {
int j = (i < 2) ? 0 : 1; int j = (i < 2) ? 0 : 1;
a(0) = static_cast<float>((i % 2)*(nx)); a(0) = static_cast<float>((i % 2)*(nx));
@ -233,8 +233,8 @@ void RectifyProcessor::stereoRectify(models::CameraPtr leftOdo,
// Eigen::Matrix4d RectifyProcessor::loadT(const mynteye::Extrinsics& in) { // Eigen::Matrix4d RectifyProcessor::loadT(const mynteye::Extrinsics& in) {
// subEigen // subEigen
Ctain::Matrix4d RectifyProcessor::loadT(const mynteye::Extrinsics &in) { ctain::Matrix4d RectifyProcessor::loadT(const mynteye::Extrinsics &in) {
Ctain::Matrix3d R(3); ctain::Matrix3d R(3);
R<< R<<
in.rotation[0][0] << in.rotation[0][1] << in.rotation[0][2] << in.rotation[0][0] << in.rotation[0][1] << in.rotation[0][2] <<
in.rotation[1][0] << in.rotation[1][1] << in.rotation[1][2] << in.rotation[1][0] << in.rotation[1][1] << in.rotation[1][2] <<
@ -244,12 +244,12 @@ Ctain::Matrix4d RectifyProcessor::loadT(const mynteye::Extrinsics &in) {
double t_y = in.translation[1]; double t_y = in.translation[1];
double t_z = in.translation[2]; double t_z = in.translation[2];
Ctain::Quaterniond q(R); ctain::Quaterniond q(R);
q.normalize(); q.normalize();
Ctain::Matrix4d T(4); ctain::Matrix4d T(4);
T(3, 3) = 1; T(3, 3) = 1;
T.topLeftCorner<3, 3>() = q.toRotationMatrix(); T.topLeftCorner<3, 3>() = q.toRotationMatrix();
Ctain::Vector3d t(3, 1); ctain::Vector3d t(3, 1);
t << t_x << t_y << t_z; t << t_x << t_y << t_z;
T.topRightCorner<3, 1>() = t; T.topRightCorner<3, 1>() = t;
@ -293,10 +293,10 @@ std::shared_ptr<struct CameraROSMsgInfoPair> RectifyProcessor::stereoRectify(
// Eigen::Matrix4d T = loadT(ex_right_to_left); // Eigen::Matrix4d T = loadT(ex_right_to_left);
// Eigen::Matrix3d R = T.topLeftCorner<3, 3>(); // Eigen::Matrix3d R = T.topLeftCorner<3, 3>();
// Eigen::Vector3d t = T.topRightCorner<3, 1>(); // Eigen::Vector3d t = T.topRightCorner<3, 1>();
Ctain::Matrix4d T = loadT(ex_right_to_left); ctain::Matrix4d T = loadT(ex_right_to_left);
Ctain::Matrix3d R; ctain::Matrix3d R;
R = T.topLeftCorner<3, 3>(); R = T.topLeftCorner<3, 3>();
Ctain::Vector3d t = T.topRightCorner<3, 1>(); ctain::Vector3d t = T.topRightCorner<3, 1>();
// cv::Mat cv_R, cv_t; // cv::Mat cv_R, cv_t;
// cv::eigen2cv(R, cv_R); // cv::eigen2cv(R, cv_R);
cv::Mat cv_R(3, 3, CV_64FC1); cv::Mat cv_R(3, 3, CV_64FC1);

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

@ -92,7 +92,7 @@ class RectifyProcessor : public Processor {
// Eigen::Matrix4d loadT(const mynteye::Extrinsics& in); // Eigen::Matrix4d loadT(const mynteye::Extrinsics& in);
// subEigen // subEigen
Ctain::Matrix4d loadT(const mynteye::Extrinsics &in); ctain::Matrix4d loadT(const mynteye::Extrinsics &in);
void loadCameraMatrix(cv::Mat& K, cv::Mat& D, // NOLINT void loadCameraMatrix(cv::Mat& K, cv::Mat& D, // NOLINT
cv::Size& image_size, // NOLINT cv::Size& image_size, // NOLINT