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

Merge branch 'rmEigen' of http://gitlab.mynt.com/mynteye/mynt-eye-s-sdk into rmEigen

Browse Source
This commit is contained in:
TinyO 2019-09-05 16:27:01 +08:00
commit 789a6f4a0e
12 changed files with 85 additions and 84 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -139,6 +139,7 @@ if(WITH_CAM_MODELS)
${CMAKE_CURRENT_BINARY_DIR}/include
${EIGEN_INCLUDE_DIR}
src/mynteye/api/camera_models
src/mynteye
)
add_library(camera_models STATIC

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

@ -104,7 +104,7 @@ void Camera::estimateExtrinsics(
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);
models::Vectord P(3, 1), p(2, 1);
p<< imagePoints.at(i).x << imagePoints.at(i).y;
// liftProjective(
@ -121,8 +121,8 @@ void Camera::estimateExtrinsics(
}
double Camera::reprojectionDist(
const ctain::Vector3d &P1, const ctain::Vector3d &P2) const {
ctain::Vector2d p1(2, 1), p2(2, 1);
const models::Vector3d &P1, const models::Vector3d &P2) const {
models::Vector2d p1(2, 1), p2(2, 1);
spaceToPlane(P1, p1);
spaceToPlane(P2, p2);
@ -170,13 +170,13 @@ 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 {
ctain::Vector3d P_cam;
const models::Vector3d &P, const models::Quaterniond &camera_q,
const models::Vector3d &camera_t,
const models::Vector2d &observed_p) const {
models::Vector3d P_cam;
P_cam = camera_q.toRotationMatrix() * P + camera_t;
ctain::Vector2d p(2, 1), res(2, 1);
models::Vector2d p(2, 1), res(2, 1);
spaceToPlane(P_cam, p);
res = p - observed_p;
return res.norm();
@ -192,24 +192,24 @@ void Camera::projectPoints(
cv::Mat R0;
cv::Rodrigues(rvec, R0);
ctain::MatrixXd R(3, 3);
models::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);
ctain::Vectord t(3, 1);
models::Vectord t(3, 1);
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
ctain::Vectord P(3, 1);
models::Vectord P(3, 1);
P << objectPoint.x << objectPoint.y << objectPoint.z;
P = R * P + t;
ctain::Vector2d p(2, 1);
models::Vector2d p(2, 1);
spaceToPlane(P, p);
imagePoints.push_back(cv::Point2f(p(0), p(1)));

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

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

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

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

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

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

4
src/mynteye/api/camera_models/util/base.h
View File

@ -24,7 +24,7 @@
MYNTEYE_BEGIN_NAMESPACE
namespace ctain {
namespace models {
typedef SMatrix<double> Matrixd;
typedef Matrix<double> MatrixXd;
typedef Matrix<double> Matrix23d;
@ -45,7 +45,7 @@ namespace ctain {
typedef Matrix<double> MatrixXcd;
typedef Quaternion<double> Quaterniond;
} // namespace ctain
} // namespace models
MYNTEYE_END_NAMESPACE
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_CTAINBASE_H_

4
src/mynteye/api/camera_models/util/matrix.h
View File

@ -23,7 +23,7 @@
MYNTEYE_BEGIN_NAMESPACE
namespace ctain {
namespace models {
template<typename _Scalar>
class Matrix {
public:
@ -419,7 +419,7 @@ double Matrix<_Scalar>::norm(void) const {
return sum;
}
} // namespace ctain
} // namespace models
MYNTEYE_END_NAMESPACE
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_MATRIX_H_

10
src/mynteye/api/camera_models/util/matrix_solver.h
View File

@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#pragma once
#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_CTAIN_MATRIX_SOLVER_H_
#define SRC_MYNTEYE_API_CAMERA_MODELS_CTAIN_MATRIX_SOLVER_H_
#ifndef SRC_MYNTEYE_API_CAMERA_MODELS_models_MATRIX_SOLVER_H_
#define SRC_MYNTEYE_API_CAMERA_MODELS_models_MATRIX_SOLVER_H_
#include <cmath>
#include <complex>
#include "mynteye/mynteye.h"
@ -24,7 +24,7 @@ static bool Matrix_EigenValue(double *k1, int n,
int loop_number, double error1, double *ret);
static void Matrix_Hessenberg(double *a1, int n, double *ret);
namespace ctain {
namespace models {
class EigenSolver {
public:
explicit EigenSolver(SMatrix<double> s) {
@ -48,7 +48,7 @@ class EigenSolver {
Matrix<double> t;
};
} // namespace ctain
} // namespace models
static void Matrix_Hessenberg(double *a1, int n, double *ret) {
int MaxNumber;
@ -253,4 +253,4 @@ static bool Matrix_EigenValue(double *k1, int n,
}
MYNTEYE_END_NAMESPACE
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_CTAIN_MATRIX_SOLVER_H_
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_models_MATRIX_SOLVER_H_

12
src/mynteye/api/camera_models/util/matrix_square.h
View File

@ -19,7 +19,7 @@
MYNTEYE_BEGIN_NAMESPACE
namespace ctain {
namespace models {
#define Matrix_ Matrix<_Scalar>
template<typename _Scalar>
class SMatrix: public Matrix_{
@ -30,8 +30,8 @@ class SMatrix: public Matrix_{
SMatrix(_Scalar **_data, int dim) : Matrix_(_data, dim, dim) {}
explicit SMatrix(Matrix_ m) : Matrix_(m) {}
_Scalar determinant(void) const;
_Scalar M(int m, int n);
SMatrix<_Scalar> inverse(void);
_Scalar M(int m, int n) const;
SMatrix<_Scalar> inverse(void) const;
void operator =(Matrix<_Scalar> m) {
SMatrix t(m);
*this = t;
@ -39,7 +39,7 @@ class SMatrix: public Matrix_{
};
template<typename _Scalar>
SMatrix<_Scalar> inverse(void) const{
SMatrix<_Scalar> SMatrix<_Scalar>::inverse(void) const{
SMatrix<_Scalar> res(Matrix_::_Rows);
_Scalar d = determinant();
for (int i = 0; i < Matrix_::_Rows; i++) {
@ -80,7 +80,7 @@ _Scalar SMatrix<_Scalar>::determinant(void) const{
}
template<typename _Scalar>
_Scalar SMatrix<_Scalar>::M(int m, int n) {
_Scalar SMatrix<_Scalar>::M(int m, int n) const {
float mid_result = 0;
int sign = 1;
int k = Matrix_::_Rows;
@ -99,7 +99,7 @@ _Scalar SMatrix<_Scalar>::M(int m, int n) {
}
#undef Matrix_
} // namespace ctain
} // namespace models
MYNTEYE_END_NAMESPACE
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_SQUAREMATRIX_H_

4
src/mynteye/api/camera_models/util/quaternion.h
View File

@ -21,7 +21,7 @@
MYNTEYE_BEGIN_NAMESPACE
namespace ctain {
namespace models {
template<typename T>
class Quaternion {
public:
@ -66,7 +66,7 @@ class Quaternion {
T _z;
T _w;
};
} // namespace ctain
} // namespace models
MYNTEYE_END_NAMESPACE
#endif // SRC_MYNTEYE_API_CAMERA_MODELS_QUATERNION_H_

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);
// Eigen::Vector2d a;
// Eigen::Vector3d b;
ctain::Vector2d a(2, 1);
ctain::Vector3d b(3, 1);
models::Vector2d a(2, 1);
models::Vector3d b(3, 1);
for (i = 0; i < 4; i++) {
int j = (i < 2) ? 0 : 1;
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) {
// subEigen
ctain::Matrix4d RectifyProcessor::loadT(const mynteye::Extrinsics &in) {
ctain::Matrix3d R(3);
models::Matrix4d RectifyProcessor::loadT(const mynteye::Extrinsics &in) {
models::Matrix3d R(3);
R<<
in.rotation[0][0] << in.rotation[0][1] << in.rotation[0][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_z = in.translation[2];
ctain::Quaterniond q(R);
models::Quaterniond q(R);
q.normalize();
ctain::Matrix4d T(4);
models::Matrix4d T(4);
T(3, 3) = 1;
T.topLeftCorner<3, 3>() = q.toRotationMatrix();
ctain::Vector3d t(3, 1);
models::Vector3d t(3, 1);
t << t_x << t_y << t_z;
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::Matrix3d R = T.topLeftCorner<3, 3>();
// Eigen::Vector3d t = T.topRightCorner<3, 1>();
ctain::Matrix4d T = loadT(ex_right_to_left);
ctain::Matrix3d R;
models::Matrix4d T = loadT(ex_right_to_left);
models::Matrix3d R;
R = T.topLeftCorner<3, 3>();
ctain::Vector3d t = T.topRightCorner<3, 1>();
models::Vector3d t = T.topRightCorner<3, 1>();
// cv::Mat cv_R, cv_t;
// cv::eigen2cv(R, cv_R);
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);
// subEigen
ctain::Matrix4d loadT(const mynteye::Extrinsics &in);
models::Matrix4d loadT(const mynteye::Extrinsics &in);
void loadCameraMatrix(cv::Mat& K, cv::Mat& D, // NOLINT
cv::Size& image_size, // NOLINT