#include <visp3/core/vpConfig.h>
#include <visp3/core/vpDebug.h>
#include <visp3/core/vpGEMM.h>
#include <visp3/core/vpHomogeneousMatrix.h>
#include <visp3/core/vpMath.h>
#include <visp3/core/vpVelocityTwistMatrix.h>
#include <stdio.h>
#include <stdlib.h>
namespace
{
bool test(
const std::string &s,
const vpMatrix &M,
const std::vector<double> &bench)
{
static unsigned int cpt = 0;
std::cout << "** Test " << ++cpt << std::endl;
std::cout << s <<
"(" << M.
getRows() <<
"," << M.
getCols() <<
") = \n" << M << std::endl;
if (bench.size() != M.
size()) {
std::cout << "Test fails: bad size wrt bench" << std::endl;
return false;
}
for (
unsigned int i = 0; i < M.
size(); i++) {
if (std::fabs(M.
data[i] - bench[i]) > std::fabs(M.
data[i]) * std::numeric_limits<double>::epsilon()) {
std::cout << "Test fails: bad content" << std::endl;
return false;
}
}
return true;
}
double getRandomValues(const double min, const double max)
{
return (max - min) * ((double)rand() / (double)RAND_MAX) + min;
}
bool equalMatrix(
const vpMatrix &A,
const vpMatrix &B,
const double tol = std::numeric_limits<double>::epsilon())
{
return false;
}
for (
unsigned int i = 0; i < A.
getRows(); i++) {
for (
unsigned int j = 0; j < A.
getCols(); j++) {
return false;
}
}
}
return true;
}
vpMatrix generateRandomMatrix(
const unsigned int rows,
const unsigned int cols,
const double min,
const double max)
{
for (
unsigned int i = 0; i < M.
getRows(); i++) {
for (
unsigned int j = 0; j < M.
getCols(); j++) {
M[i][j] = getRandomValues(min, max);
}
}
return M;
}
#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
vpColVector generateRandomVector(
const unsigned int rows,
const double min,
const double max)
{
for (
unsigned int i = 0; i < v.
getRows(); i++) {
v[i] = getRandomValues(min, max);
}
return v;
}
{
}
unsigned int BcolNum = B.
getCols();
unsigned int BrowNum = B.
getRows();
unsigned int i, j, k;
for (i = 0; i < A.
getRows(); i++) {
double *rowptri = A[i];
double *ci = C[i];
for (j = 0; j < BcolNum; j++) {
double s = 0;
for (k = 0; k < BrowNum; k++)
s += rowptri[k] * B[k][j];
ci[j] = s;
}
}
return C;
}
{
unsigned int i, j, k;
double s;
double *ptr;
for (i = 0; i < A.
getCols(); i++) {
double *Bi = B[i];
for (j = 0; j < i; j++) {
s = 0;
for (k = 0; k < A.
getRows(); k++) {
s += (*(ptr + i)) * (*(ptr + j));
}
*Bi++ = s;
B[j][i] = s;
}
s = 0;
for (k = 0; k < A.
getRows(); k++) {
s += (*(ptr + i)) * (*(ptr + i));
}
*Bi = s;
}
return B;
}
{
}
for (
unsigned int j = 0; j < A.
getCols(); j++) {
double vj = v[j];
for (
unsigned int i = 0; i < A.
getRows(); i++) {
w[i] += A[i][j] * vj;
}
}
return w;
}
{
}
unsigned int VcolNum = V.
getCols();
unsigned int VrowNum = V.
getRows();
for (
unsigned int i = 0; i < A.
getRows(); i++) {
double *rowptri = A[i];
double *ci = M[i];
for (unsigned int j = 0; j < VcolNum; j++) {
double s = 0;
for (unsigned int k = 0; k < VrowNum; k++)
s += rowptri[k] * V[k][j];
ci[j] = s;
}
}
return M;
}
#endif
}
int main(int argc, char *argv[])
{
try {
bool ctest = true;
for (int i = 1; i < argc; i++) {
if (std::string(argv[i]) == "--benchmark") {
ctest = false;
}
}
{
const double val = 10.0;
M = val;
for (
unsigned int i = 0; i < M.
getRows(); i++) {
for (
unsigned int j = 0; j < M.
getCols(); j++) {
if (!
vpMath::equal(M[i][j], val, std::numeric_limits<double>::epsilon())) {
std::cerr << "Issue with matrix assignment with value." << std::endl;
return EXIT_FAILURE;
}
if (!
vpMath::equal(M2[i][j], val, std::numeric_limits<double>::epsilon())) {
std::cerr << "Issue with matrix constructor initialized with value." << std::endl;
return EXIT_FAILURE;
}
}
}
}
{
std::vector<double> bench(9, 0);
bench[2] = bench[4] = bench[6] = 1.;
M[2][0] = M[1][1] = M[0][2] = 1.;
if (test("R1", R1, bench) == false)
return EXIT_FAILURE;
R2 = M;
if (test("R2", R2, bench) == false)
return EXIT_FAILURE;
}
{
std::vector<double> bench(6, 1);
if (test("M1", M1, bench) == false)
return EXIT_FAILURE;
if (test("M2", M2, bench) == false)
return EXIT_FAILURE;
}
{
int val = 0;
for (
unsigned int i = 0; i < M.
getRows(); i++) {
for (
unsigned int j = 0; j < M.
getCols(); j++) {
M[i][j] = val++;
}
}
std::cout << "M ";
std::cout << "N ";
std::string header("My 4-by-5 matrix\nwith a second line");
std::cout << "Matrix saved in matrix.mat file" << std::endl;
else
return EXIT_FAILURE;
char header_[100];
std::cout << "Matrix loaded from matrix.mat file with header \"" << header_ << "\": \n" << M1 << std::endl;
else
return EXIT_FAILURE;
if (header != std::string(header_)) {
std::cout << "Bad header in matrix.mat" << std::endl;
return EXIT_FAILURE;
}
std::cout << "Matrix saved in matrix.bin file" << std::endl;
else
return EXIT_FAILURE;
std::cout << "Matrix loaded from matrix.bin file with header \"" << header_ << "\": \n" << M1 << std::endl;
else
return EXIT_FAILURE;
if (header != std::string(header_)) {
std::cout << "Bad header in matrix.bin" << std::endl;
return EXIT_FAILURE;
}
std::cout << "Matrix saved in matrix.yml file" << std::endl;
else
return EXIT_FAILURE;
std::cout << "Matrix loaded from matrix.yml file with header \"" << header_ << "\": \n" << M2 << std::endl;
else
return EXIT_FAILURE;
if (header != std::string(header_)) {
std::cout << "Bad header in matrix.mat" << std::endl;
return EXIT_FAILURE;
}
}
{
std::cout << "R: \n" << R << std::endl;
std::cout << "M1: \n" << M1 << std::endl;
std::cout << "M2: \n" << M2 << std::endl;
std::cout << "M3: \n" << M3 << std::endl;
std::cout << "M4: \n" << M4 << std::endl;
}
{
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST PRETTY PRINT---" << std::endl;
std::cout << "------------------------" << std::endl;
std::cout << "call std::cout << M;" << std::endl;
std::cout << M << std::endl;
std::cout << "call M.print (std::cout, 4);" << std::endl;
std::cout << "------------------------" << std::endl;
M[1][0] = 1.235;
M[1][1] = 12.345;
M[1][2] = .12345;
std::cout << "call std::cout << M;" << std::endl;
std::cout << M;
std::cout << "call M.print (std::cout, 6);" << std::endl;
std::cout << std::endl;
std::cout << "------------------------" << std::endl;
M[0][0] = -1.235;
M[1][0] = -12.235;
std::cout << "call std::cout << M;" << std::endl;
std::cout << M << std::endl;
std::cout << "call M.print (std::cout, 10);" << std::endl;
M.print(std::cout, 10);
std::cout << std::endl;
std::cout << "call M.print (std::cout, 2);" << std::endl;
M.print(std::cout, 2);
std::cout << std::endl;
std::cout << "------------------------" << std::endl;
M.resize(3, 3);
M.eye(3);
M[0][2] = -0.0000000876;
std::cout << "call std::cout << M;" << std::endl;
std::cout << M << std::endl;
std::cout << "call M.print (std::cout, 4);" << std::endl;
M.print(std::cout, 4);
std::cout << std::endl;
std::cout << "call M.print (std::cout, 10, \"M\");" << std::endl;
M.print(std::cout, 10, "M");
std::cout << std::endl;
std::cout << "call M.print (std::cout, 20, \"M\");" << std::endl;
M.print(std::cout, 20, "M");
std::cout << std::endl;
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST RESIZE --------" << std::endl;
std::cout << "------------------------" << std::endl;
std::cout << "5x5" << std::endl;
M.resize(5, 5, false);
std::cout << M << std::endl;
std::cout << "3x2" << std::endl;
M.resize(3, 2, false);
std::cout << M << std::endl;
std::cout << "2x2" << std::endl;
M.resize(2, 2, false);
std::cout << M << std::endl;
std::cout << "------------------------" << std::endl;
A = 1.0;
B = A * vMe;
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST vpRowVector * vpColVector" << std::endl;
std::cout << "------------------------" << std::endl;
r[0] = 2;
r[1] = 3;
r[2] = 4;
c[0] = 1;
c[1] = 2;
c[2] = -1;
double rc = r * c;
r.
print(std::cout, 2,
"r");
c.print(std::cout, 2, "c");
std::cout << "r * c = " << rc << std::endl;
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST vpRowVector * vpMatrix" << std::endl;
std::cout << "------------------------" << std::endl;
M.resize(3, 3);
M.eye(3);
M[1][0] = 1.5;
M[2][0] = 2.3;
r.
print(std::cout, 2,
"r");
M.print(std::cout, 10, "M");
std::cout << "r * M = " << rM << std::endl;
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST vpGEMM " << std::endl;
std::cout << "------------------------" << std::endl;
M.eye(3);
N[0][0] = 2;
N[1][0] = 1.2;
N[1][2] = 0.6;
N[2][2] = 0.25;
vpGEMM(M, N, 2, C, 3, D, VP_GEMM_A_T);
std::cout << D << std::endl;
}
{
std::cout << "------------------------" << std::endl;
std::cout << "--- TEST vpMatrix insert() with same colNum " << std::endl;
std::cout << "------------------------" << std::endl;
const unsigned int nb = ctest ? 10 : 100;
const unsigned int size = ctest ? 10 : 100;
std::vector<vpMatrix> submatrices(nb);
for (size_t cpt = 0; cpt < submatrices.size(); cpt++) {
for (
unsigned int i = 0; i < m.
getRows(); i++) {
for (
unsigned int j = 0; j < m.
getCols(); j++) {
m[i][j] = getRandomValues(-100.0, 100.0);
}
}
submatrices[cpt] = m;
}
for (unsigned int i = 0; i < nb; i++) {
m_big.
insert(submatrices[(
size_t)i], i * size, 0);
}
std::cout << "Matrix insert(): " << t << " ms" << std::endl;
for (unsigned int cpt = 0; cpt < nb; cpt++) {
for (unsigned int i = 0; i < size; i++) {
for (unsigned int j = 0; j < 6; j++) {
if (!
vpMath::equal(m_big[cpt * size + i][j], submatrices[(
size_t)cpt][i][j],
std::numeric_limits<double>::epsilon())) {
std::cerr << "Problem with vpMatrix insert()!" << std::endl;
return EXIT_FAILURE;
}
}
}
}
std::cout << "Insert empty matrices:" << std::endl;
std::cout << "m1:\n" << m1 << std::endl;
std::cout << "m2:\n" << m2 << std::endl;
std::cout << "m3:\n" << m3 << std::endl;
std::cout << "\n------------------------" << std::endl;
std::cout << "--- TEST vpMatrix stack()" << std::endl;
std::cout << "------------------------" << std::endl;
{
L2 = 2;
std::cout << "L:\n" << L << std::endl;
L2 = 3;
std::cout << "L:\n" << L << std::endl;
}
{
for (unsigned int i = 0; i < nb; i++) {
m_big_stack.
stack(submatrices[(
size_t)i]);
}
std::cout << "Matrix stack(): " << t << " ms" << std::endl;
if (!equalMatrix(m_big, m_big_stack)) {
std::cerr << "Problem with vpMatrix stack()!" << std::endl;
return EXIT_FAILURE;
}
}
std::cout << "\n------------------------" << std::endl;
std::cout << "--- TEST vpMatrix stack(vpRowVector)" << std::endl;
std::cout << "------------------------" << std::endl;
vpMatrix m_big_stack = generateRandomMatrix(10000, ctest ? 10 : 100, -1000.0, 1000.0);
std::cout <<
"m_big_stack: " << m_big_stack.
getRows() <<
"x" << m_big_stack.
getCols() << std::endl;
for (
unsigned int i = 0; i < m_big_stack.
getRows(); i++) {
}
std::cout << "Matrix stack(vpRowVector): " << t << " ms" << std::endl;
if (!equalMatrix(m_big_stack, m_big_stack_row)) {
std::cerr << "Problem with vpMatrix stack(vpRowVector)!" << std::endl;
return EXIT_FAILURE;
}
std::cout << "\n------------------------" << std::endl;
std::cout << "--- TEST vpMatrix stack(vpColVector)" << std::endl;
std::cout << "------------------------" << std::endl;
for (
unsigned int j = 0; j < m_big_stack.
getCols(); j++) {
}
std::cout << "Matrix stack(vpColVector): " << t << " ms" << std::endl;
if (!equalMatrix(m_big_stack, m_big_stack_col)) {
std::cerr << "Problem with vpMatrix stack(vpColVector)!" << std::endl;
return EXIT_FAILURE;
}
std::cout << "\n------------------------" << std::endl;
std::cout << "--- TEST vpMatrix::stack()" << std::endl;
std::cout << "------------------------" << std::endl;
{
L2 = 2;
std::cout << "L:\n" << L << std::endl;
L2.resize(3, 6);
L2 = 3;
L_tmp = L;
std::cout << "L:\n" << L << std::endl;
}
{
vpMatrix m_big_stack_static, m_big_stack_static_tmp;
for (unsigned int i = 0; i < nb; i++) {
vpMatrix::stack(m_big_stack_static_tmp, submatrices[(
size_t)i], m_big_stack_static);
m_big_stack_static_tmp = m_big_stack_static;
}
std::cout << "Matrix::stack(): " << t << " ms" << std::endl;
if (!equalMatrix(m_big, m_big_stack_static)) {
std::cerr << "Problem with vpMatrix::stack()!" << std::endl;
return EXIT_FAILURE;
}
}
std::cout << "\n------------------------" << std::endl;
std::cout << "--- TEST vpMatrix::stack(vpMatrix, vpRowVector, vpMatrix)" << std::endl;
std::cout << "------------------------" << std::endl;
vpMatrix m_big_stack_static = generateRandomMatrix(ctest ? 100 : 1000, ctest ? 10 : 100, -1000.0, 1000.0);
std::cout <<
"m_big_stack_static: " << m_big_stack_static.
getRows() <<
"x" << m_big_stack_static.
getCols() << std::endl;
vpMatrix m_big_stack_static_row, m_big_stack_static_row_tmp;
for (
unsigned int i = 0; i < m_big_stack_static.
getRows(); i++) {
m_big_stack_static_row_tmp = m_big_stack_static_row;
}
std::cout << "Matrix::stack(vpMatrix, vpRowVector, vpMatrix): " << t << " ms" << std::endl;
if (!equalMatrix(m_big_stack_static, m_big_stack_static_row)) {
std::cerr << "Problem with vpMatrix::stack(vpMatrix, vpRowVector, "
"vpMatrix)!"
<< std::endl;
return EXIT_FAILURE;
}
std::cout << "\n------------------------" << std::endl;
std::cout << "--- TEST vpMatrix::stack(vpMatrix, vpColVector, vpMatrix)" << std::endl;
std::cout << "------------------------" << std::endl;
vpMatrix m_big_stack_static_col, m_big_stack_static_col_tmp;
for (
unsigned int j = 0; j < m_big_stack_static.
getCols(); j++) {
m_big_stack_static_col_tmp = m_big_stack_static_col;
}
std::cout << "Matrix::stack(vpMatrix, vpColVector, vpMatrix): " << t << " ms" << std::endl;
if (!equalMatrix(m_big_stack_static, m_big_stack_static_col)) {
std::cerr << "Problem with vpMatrix::stack(vpMatrix, vpColVector, "
"vpMatrix)!"
<< std::endl;
return EXIT_FAILURE;
}
}
{
for (
unsigned int i = 0; i < m2.
getRows(); i++) {
for (
unsigned int j = 0; j < m2.
getCols(); j++) {
m2[i][j] = getRandomValues(-100.0, 100.0);
}
}
unsigned int offset_i = 4, offset_j = 3;
m1.insert(m2, offset_i, offset_j);
for (
unsigned int i = 0; i < m2.
getRows(); i++) {
for (
unsigned int j = 0; j < m2.
getCols(); j++) {
if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
std::cerr << "Problem with vpMatrix insert()!" << std::endl;
return EXIT_FAILURE;
}
}
}
offset_i = 4, offset_j = 5;
m1.insert(m2, offset_i, offset_j);
for (
unsigned int i = 0; i < m2.
getRows(); i++) {
for (
unsigned int j = 0; j < m2.
getCols(); j++) {
if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
std::cerr << "Problem with vpMatrix insert()!" << std::endl;
return EXIT_FAILURE;
}
}
}
offset_i = 8, offset_j = 5;
m1.insert(m2, offset_i, offset_j);
for (
unsigned int i = 0; i < m2.
getRows(); i++) {
for (
unsigned int j = 0; j < m2.
getCols(); j++) {
if (!
vpMath::equal(m1[i + offset_i][j + offset_j], m2[i][j], std::numeric_limits<double>::epsilon())) {
std::cerr << "Problem with vpMatrix insert()!" << std::endl;
return EXIT_FAILURE;
}
}
}
}
{
std::cout << "\n------------------------" << std::endl;
std::cout << "--- TEST vpMatrix::juxtaposeMatrices()" << std::endl;
std::cout << "------------------------" << std::endl;
for (unsigned int i = 0; i < A.getRows(); i++) {
for (unsigned int j = 0; j < A.getCols(); j++) {
A[i][j] = i * A.getCols() + j;
B[i][j] = (i * B.
getCols() + j) * 10;
}
}
}
std::cout << "juxtaposeM:\n" << juxtaposeM << std::endl;
}
#if defined(VISP_HAVE_LAPACK) && !defined(VISP_HAVE_LAPACK_BUILT_IN)
{
std::cout << "\n------------------------" << std::endl;
std::cout << "--- BENCHMARK dgemm/dgemv" << std::endl;
std::cout << "------------------------" << std::endl;
size_t nb_matrices = ctest ? 100 : 10000;
unsigned int rows = 200, cols = 6;
double min = -1.0, max = 1.0;
std::vector<vpMatrix> vec_A, vec_B, vec_C, vec_C_regular;
vec_C.reserve(nb_matrices);
vec_C_regular.reserve(nb_matrices);
for (size_t i = 0; i < nb_matrices; i++) {
vec_A.push_back(generateRandomMatrix(cols, rows, min, max));
vec_B.push_back(generateRandomMatrix(rows, cols, min, max));
}
for (size_t i = 0; i < nb_matrices; i++) {
vec_C.push_back(vec_A[i] * vec_B[i]);
}
std::cout << nb_matrices << " matrix multiplication: (6x200) x (200x6)" << std::endl;
std::cout << "Lapack: " << t << " ms" << std::endl;
std::cout << "vec_C:\n" << vec_C.back() << std::endl;
for (size_t i = 0; i < nb_matrices; i++) {
vec_C_regular.push_back(dgemm_regular(vec_A[i], vec_B[i]));
}
std::cout << "\nRegular: " << t << " ms" << std::endl;
std::cout << "vec_C_regular:\n" << vec_C_regular.back() << std::endl;
vpMatrix A = generateRandomMatrix(480, 640, min, max), B = generateRandomMatrix(640, 480, min, max);
AB = A * B;
std::cout << "\nMatrix multiplication: (480x640) x (640x480)" << std::endl;
std::cout << "Lapack: " << t << " ms" << std::endl;
AB_regular = dgemm_regular(A, B);
std::cout << "Regular: " << t << " ms" << std::endl;
bool res = equalMatrix(AB, AB_regular, 1e-9);
std::cout << "Check result: " << res << std::endl;
if (!res) {
std::cerr << "Problem with matrix multiplication!" << std::endl;
return EXIT_FAILURE;
}
int nb_iterations = 1000;
vpMatrix L = generateRandomMatrix(1000, 6, min, max);
for (int i = 0; i < nb_iterations; i++)
std::cout << "\n" << nb_iterations << " iterations of AtA for size: (1000x6)" << std::endl;
std::cout << "Lapack: " << t << " ms" << std::endl;
std::cout << "LTL:\n" << LTL << std::endl;
for (int i = 0; i < nb_iterations; i++)
LTL_regular = AtA_regular(L);
std::cout << "\nRegular: " << t << " ms" << std::endl;
std::cout << "LTL_regular:\n" << LTL_regular << std::endl;
res = equalMatrix(LTL, LTL_regular, 1e-9);
std::cout << "Check result: " << res << std::endl;
if (!res) {
std::cerr << "Problem with vpMatrix::AtA()!" << std::endl;
return EXIT_FAILURE;
}
vpMatrix LT = generateRandomMatrix(6, 1000, min, max);
for (int i = 0; i < nb_iterations; i++)
LTR = LT * R;
std::cout << "\n"
<< nb_iterations
<< " iterations of matrix vector multiplication: (6x1000) x "
"(1000x1)"
<< std::endl;
std::cout << "Lapack: " << t << " ms" << std::endl;
std::cout <<
"LTR:\n" << LTR.
t() << std::endl;
for (int i = 0; i < nb_iterations; i++)
LTR_regular = dgemv_regular(LT, R);
std::cout << "\nRegular: " << t << " ms" << std::endl;
std::cout <<
"LTR_regular:\n" << LTR_regular.
t() << std::endl;
res = equalMatrix(LTR, LTR_regular, 1e-9);
std::cout << "Check result: " << res << std::endl;
if (!res) {
std::cerr << "Problem with dgemv!" << std::endl;
return EXIT_FAILURE;
}
getRandomValues(min, max), getRandomValues(min, max), getRandomValues(min, max));
for (int i = 0; i < nb_iterations; i++)
LV = L * V;
std::cout << "\n"
<< nb_iterations
<< " iterations of matrix velocity twist matrix "
"multiplication: (1000x6) x (6x6)"
<< std::endl;
std::cout << "Lapack: " << t << " ms" << std::endl;
for (int i = 0; i < nb_iterations; i++)
LV_regular = mat_mul_twist_matrix(L, V);
std::cout << "Regular: " << t << " ms" << std::endl;
res = equalMatrix(LV, LV_regular, 1e-9);
std::cout << "Check result: " << res << std::endl;
if (!res) {
std::cerr << "Problem with matrix and velocity twist matrix multiplication!" << std::endl;
return EXIT_FAILURE;
}
}
#endif
#ifdef VISP_HAVE_CPP11_COMPATIBILITY
{
std::vector<vpMatrix> vec_mat;
vec_mat.emplace_back(5, 5);
A = 1;
B = 2;
std::cout << "\n1) A+B:\n" << res << std::endl;
res2 = A + B;
std::cout << "\n2) A+B:\n" << res2 << std::endl;
}
#endif
{
std::cout << "\n------------------------" << std::endl;
std::cout << "--- TEST vpMatrix::hadamard()" << std::endl;
std::cout << "------------------------" << std::endl;
for (unsigned int i = 0; i < M1.size(); i++) {
M1.data[i] = i;
}
std::cout << "M1:\n" << M1 << std::endl;
std::cout << "\nM2:\n" << M2 << std::endl;
std::cout << "\nRes:\n" << M2 << std::endl;
}
std::cout << "\nAll tests succeed" << std::endl;
return EXIT_SUCCESS;
std::cout << "Catch an exception: " << e << std::endl;
return EXIT_FAILURE;
}
}