20 #include "../base.hpp" 22 #include "../util/matrix_3_x_3.hpp" 23 #include "../util/vec.hpp" 25 #ifndef PIC_DISABLE_EIGEN 27 #ifndef PIC_EIGEN_NOT_BUNDLED 28 #include "../externals/Eigen/LU" 29 #include "../externals/Eigen/Geometry" 32 #include <Eigen/Geometry> 37 #ifndef PIC_EIGEN_UTIL 38 #define PIC_EIGEN_UTIL 40 #ifndef PIC_DISABLE_EIGEN 42 typedef Matrix<float, 3, 4> Matrix34f;
43 typedef Matrix<double, 3, 4> Matrix34d;
49 #ifndef PIC_DISABLE_EIGEN 60 FILE *file = fopen(nameFile.c_str(),
"r");
66 for(
int i = 0; i < 3; i++) {
67 for(
int j = 0; j < 4; j++) {
69 fscanf(file,
"%f", &val);
70 mat(i, j) = double(val);
87 FILE *file = fopen(nameFile.c_str(),
"w");
93 for(
int i = 0; i < 3; i++) {
94 for(
int j = 0; j < 4; j++) {
95 fprintf(file,
"%f ", mat(i, j));
149 ret(0, 0) = mat(0, 0);
150 ret(0, 1) = mat(0, 1);
151 ret(0, 2) = mat(0, 2);
153 ret(1, 0) = mat(1, 0);
154 ret(1, 1) = mat(1, 1);
155 ret(1, 2) = mat(1, 2);
157 ret(2, 0) = mat(2, 0);
158 ret(2, 1) = mat(2, 1);
159 ret(2, 2) = mat(2, 2);
187 return Eigen::Vector3f(x[0], x[1], 1.0f);
197 return Eigen::Vector3d(x[0], x[1], 1.0);
207 return Eigen::Vector4d(x[0], x[1], x[2], 1.0);
216 printf(
"%f %f %f\n", x[0], x[1], x[2]);
226 for(
int i = 0; i < mat.rows(); i++){
227 for(
int j = 0; j < mat.cols(); j++){
228 printf(
"%3.3f ", mat(i, j));
240 for(
int i = 0; i < 3; i++){
241 for(
int j = 0; j < 3; j++){
242 printf(
"%f ", mat(i, j));
254 for(
int i = 0; i < 3; i++){
255 for(
int j = 0; j < 4; j++){
256 printf(
"%.9f ", mat(i, j));
268 FILE *file = fopen(name.c_str(),
"w");
269 for(
int i = 0; i < mat.rows(); i++){
270 for(
int j = 0; j < mat.cols(); j++){
271 fprintf(file,
"%.9f ", mat(i, j));
284 for(
int i = 0; i < 3; i++){
285 for(
int j = 0; j < 3; j++){
286 printf(
"%f ", mat(i, j));
303 double s = 1.0 / info[2];
305 ret(0,0) = s; ret(0,1) = 0.0; ret(0,2) = -cX / info[2];
306 ret(1,0) = 0.0; ret(1,1) = s; ret(1,2) = -cY / info[2];
307 ret(2,0) = 0.0; ret(2,1) = 0.0; ret(2,2) = 1.0;
320 ret(0, 0) = 0.0; ret(0, 1) = -t[2]; ret(0, 2) = t[1];
321 ret(1, 0) = t[2]; ret(1, 1) = 0.0; ret(1, 2) = -t[0];
322 ret(2, 0) = -t[1]; ret(2, 1) = t[0]; ret(2, 2) = 0.0;
335 return R * point + t;
345 Eigen::Quaternion<double> reg(R);
347 return reg.toRotationMatrix();
359 mtx.
data[0] = mat(0, 0);
360 mtx.
data[1] = mat(0, 1);
361 mtx.
data[2] = mat(0, 2);
363 mtx.
data[3] = mat(1, 0);
364 mtx.
data[4] = mat(1, 1);
365 mtx.
data[5] = mat(1, 2);
367 mtx.
data[6] = mat(2, 0);
368 mtx.
data[7] = mat(2, 1);
369 mtx.
data[8] = mat(2, 2);
384 mtx.
data[0] = float(mat(0, 0));
385 mtx.
data[1] = float(mat(0, 1));
386 mtx.
data[2] = float(mat(0, 2));
388 mtx.
data[3] = float(mat(1, 0));
389 mtx.
data[4] = float(mat(1, 1));
390 mtx.
data[5] = float(mat(1, 2));
392 mtx.
data[6] = float(mat(2, 0));
393 mtx.
data[7] = float(mat(2, 1));
394 mtx.
data[8] = float(mat(2, 2));
406 int n = int(mat.cols() * mat.rows());
408 float *ret =
new float[n];
410 for(
int i = 0; i < mat.rows(); i++) {
411 for(
int j = 0; j < mat.cols(); j++) {
412 ret[c] = float(mat(i, j));
427 int n = int(mat.cols() * mat.rows());
429 float *ret =
new float[n];
431 for(
int i = 0; i < mat.rows(); i++) {
432 for(
int j = 0; j < mat.cols(); j++) {
450 Eigen::MatrixXf ret = Eigen::MatrixXf(rows, cols);
453 for(
int i = 0; i < rows; i++) {
454 for(
int j = 0; j < cols; j++) {
455 ret(i, j) = array[c];
471 Eigen::MatrixXd ret = Eigen::MatrixXd(rows, cols);
474 for(
int i = 0; i < rows; i++) {
475 for(
int j = 0; j < cols; j++) {
476 ret(i, j) = array[c];
493 for(
int i = 0; i < 3; i++) {
494 for(
int j = 0; j < 3; j++) {
495 ret(i, j) = array[c];
510 mtx(0, 0) = mat.
data[0];
511 mtx(0, 1) = mat.
data[1];
512 mtx(0, 2) = mat.
data[2];
514 mtx(1, 0) = mat.
data[3];
515 mtx(1, 1) = mat.
data[4];
516 mtx(1, 2) = mat.
data[5];
518 mtx(2, 0) = mat.
data[6];
519 mtx(2, 1) = mat.
data[7];
520 mtx(2, 2) = mat.
data[8];
534 if(points.size() < 2) {
541 for(
unsigned int i = 0; i < points.size(); i++) {
542 ret[0] += points[i][0];
543 ret[1] += points[i][1];
546 float n = float(points.size());
551 for(
unsigned int i = 0; i < points.size(); i++) {
553 float dx = points[i][0] - ret[0];
554 float dy = points[i][1] - ret[1];
556 ret[2] += sqrtf(dx * dx + dy * dy);
559 ret[2] = ret[2] / n / sqrtf(2.0f);
570 return Vec2i(x[0], x[1]);
577 #endif // PIC_EIGEN_UTIL float data[9]
Definition: matrix_3_x_3.hpp:37
PIC_INLINE Eigen::Matrix3d getShiftScaleMatrix(Eigen::Vector3f &info)
getShiftScaleMatrix computes a shifting and scaling matrix
Definition: eigen_util.hpp:297
PIC_INLINE float * getLinearArrayFromMatrix(Eigen::Matrix3d &mat)
getLinearArray
Definition: eigen_util.hpp:404
The Matrix3x3 class provides methods for managing a 3 by 3 matrix.
Definition: matrix_3_x_3.hpp:29
PIC_INLINE Eigen::Vector3d getLastColumn(Eigen::Matrix34d &mat)
getLastColumn
Definition: eigen_util.hpp:169
PIC_INLINE Eigen::Vector3d getDiagonalFromMatrix(Eigen::Matrix3d &mat)
getDiagonalFromMatrix
Definition: eigen_util.hpp:130
PIC_INLINE void printfMat(Eigen::MatrixXd mat)
printf
Definition: eigen_util.hpp:224
PIC_INLINE Vec2i convertFromEigenToVec(Eigen::Vector2i &x)
convertFromEigenToVec
Definition: eigen_util.hpp:568
PIC_INLINE void printfMat34d(Eigen::Matrix34d &mat)
printf
Definition: eigen_util.hpp:252
Definition: eigen_util.hpp:41
Vec< 2, int > Vec2i
Vec2i.
Definition: vec.hpp:829
PIC_INLINE Eigen::Vector3f addOne(Eigen::Vector2f &x)
addOne
Definition: eigen_util.hpp:185
PIC_INLINE Eigen::MatrixXf getMatrixfFromLinearArray(float *array, int rows, int cols)
getMatrixFromLinearArray
Definition: eigen_util.hpp:448
PIC_INLINE void fprintfMat(Eigen::MatrixXd &mat, std::string name)
fprintf
Definition: eigen_util.hpp:266
PIC_INLINE Eigen::Matrix34d readMatrix34dFromFile(std::string nameFile)
readMatrix34dFromFile
Definition: eigen_util.hpp:56
PIC_INLINE Matrix3x3 MatrixConvert(Eigen::Matrix3f &mat)
MatrixConvert converts a matrix from a Eigen::Matrix3f representation into a Matrix3x3 representation...
Definition: eigen_util.hpp:356
#define PIC_INLINE
Definition: base.hpp:33
PIC_INLINE Eigen::Vector3d rigidTransform(Eigen::Vector3d &point, Eigen::Matrix3d &R, Eigen::Vector3d &t)
rigidTransform computes a rigidi transformation in 3D.
Definition: eigen_util.hpp:333
PIC_INLINE Eigen::Matrix3d getSquareMatrix(Eigen::Matrix34d &mat)
getSquareMatrix
Definition: eigen_util.hpp:146
PIC_INLINE Eigen::Matrix3d DiagonalMatrix(Eigen::Vector3d D)
DiagonalMatrix creates a diagonal matrix.
Definition: eigen_util.hpp:113
Definition: bilateral_separation.hpp:25
PIC_INLINE Eigen::Matrix3d CrossProduct(Eigen::Vector3d &t)
CrossProduct computes a cross product matrix from a vector.
Definition: eigen_util.hpp:317
PIC_INLINE Eigen::Vector3f ComputeNormalizationTransform(std::vector< Eigen::Vector2f > &points)
ComputeNormalizationTransform.
Definition: eigen_util.hpp:530
PIC_INLINE Eigen::MatrixXd getMatrixdFromLinearArray(float *array, int rows, int cols)
getMatrixFromLinearArray
Definition: eigen_util.hpp:469
PIC_INLINE Eigen::Matrix3d getMatrix3dFromLinearArray(float *array)
getMatrix3dFromLinearArray
Definition: eigen_util.hpp:488
PIC_INLINE Eigen::Matrix3d RotationMatrixRefinement(Eigen::Matrix3d &R)
RotationMatrixRefinement.
Definition: eigen_util.hpp:343
The Vec class.
Definition: vec.hpp:35
PIC_INLINE bool writeMatrix34dToFile(std::string nameFile, Eigen::Matrix34d &mat)
writeMatrix34dToFile
Definition: eigen_util.hpp:85
PIC_INLINE void printfVet3d(Eigen::Vector3d &x)
printfVet3d
Definition: eigen_util.hpp:214