54 #include <visp3/core/vpCPUFeatures.h>
55 #include <visp3/core/vpColVector.h>
56 #include <visp3/core/vpDebug.h>
57 #include <visp3/core/vpException.h>
58 #include <visp3/core/vpMath.h>
59 #include <visp3/core/vpRotationVector.h>
61 #if defined __SSE2__ || defined _M_X64 || (defined _M_IX86_FP && _M_IX86_FP >= 2)
62 #include <emmintrin.h>
63 #define VISP_HAVE_SSE2 1
75 for (
unsigned int i = 0; i <
rowNum; i++)
76 r[i] = (*
this)[i] + v[i];
108 for (
unsigned int i = 0; i < 3; i++)
109 s[i] = (*
this)[i] +
t[i];
122 for (
unsigned int i = 0; i <
rowNum; i++)
134 for (
unsigned int i = 0; i <
rowNum; i++)
150 "Cannot compute the dot product between column vectors "
151 "with different dimensions (%d) and (%d)",
156 for (
unsigned int i = 0; i <
rowNum; i++)
157 r += (*
this)[i] * v[i];
173 for (
unsigned int i = 0; i <
rowNum; i++) {
174 for (
unsigned int j = 0; j < v.
getCols(); j++) {
175 M[i][j] = (*this)[i] * v[j];
186 "Bad size during vpColVector (%dx1) and vpColVector "
187 "(%dx1) subtraction",
192 for (
unsigned int i = 0; i <
rowNum; i++)
193 v[i] = (*
this)[i] - m[i];
254 unsigned int rnrows = r + nrows;
263 for (
unsigned int i = r; i < rnrows; i++)
264 (*
this)[i - r] = v[i];
269 for (
unsigned int i = 0; i < v.
size(); i++)
275 for (
unsigned int i = 0; i < p.
size(); i++)
281 for (
unsigned int i = 0; i < v.
size(); i++)
288 for (
unsigned int i = 0; i < M.
getCols(); i++)
289 (*
this)[i] = M[i][j];
305 for (
unsigned int i = 0; i < M.
getRows(); i++)
306 (*
this)[i] = M[i][0];
314 for (
unsigned int i = 0; i < v.size(); i++)
322 for (
unsigned int i = 0; i < v.size(); i++)
323 (*
this)[i] = (double)(v[i]);
326 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
364 for (
unsigned int i = 0; i <
rowNum; i++)
397 for (
unsigned int i = 0; i <
rowNum; i++)
398 *(vd++) = (*d++) * x;
422 for (
unsigned int i = 0; i <
rowNum; i++)
446 for (
unsigned int i = 0; i <
rowNum; i++)
477 for (
unsigned int i = 0; i <
rowNum; i++)
478 *(vd++) = (*d++) / x;
505 resize((
unsigned int)v.size(),
false);
506 for (
unsigned int i = 0; i < v.size(); i++)
515 resize((
unsigned int)v.size(),
false);
516 for (
unsigned int i = 0; i < v.size(); i++)
517 (*
this)[i] = (float)v[i];
523 unsigned int k = v.
rowNum;
624 for (
unsigned int i = 0; i <
rowNum; i++) {
625 for (
unsigned int j = 0; j <
colNum; j++) {
687 for (
unsigned int i = 0; i <
rowNum; i++)
698 std::vector<double> v(this->
size());
700 for (
unsigned int i = 0; i < this->
size(); i++)
705 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
711 if (
this != &other) {
723 other.rowPtrs = NULL;
756 resize(static_cast<unsigned int>(list.size()),
false);
757 std::copy(list.begin(), list.end(),
data);
767 for (
unsigned int i = 0; i <
rowNum; i++) {
776 return !(*
this == v);
785 memcpy(v.data,
data,
rowNum *
sizeof(
double));
821 if (a.
data == NULL) {
824 if (b.
data == NULL) {
829 "Cannot compute the dot product between column vectors "
830 "with different dimensions (%d) and (%d)",
838 for (
unsigned int i = 0; i < a.
getRows(); i++)
839 c += *(ad++) * *(bd++);
872 if (std::fabs(sum_square) > std::numeric_limits<double>::epsilon())
873 *
this /= sqrt(sum_square);
909 if (v.
data == NULL) {
914 unsigned int nb_permutation = 1;
916 while (nb_permutation != 0) {
918 for (
unsigned int j = v.
getRows() - 1; j >= i + 1; j--) {
919 if ((tab[j] > tab[j - 1])) {
961 if (v.
data == NULL) {
966 unsigned int nb_permutation = 1;
968 while (nb_permutation != 0) {
970 for (
unsigned int j = v.
getRows() - 1; j >= i + 1; j--) {
971 if ((tab[j] < tab[j - 1])) {
1002 this->
resize(rowNum + 1,
false);
1072 unsigned int nrA = A.
getRows();
1073 unsigned int nrB = B.
getRows();
1075 if (nrA == 0 && nrB == 0) {
1091 C.
resize(nrA + nrB,
false);
1093 for (
unsigned int i = 0; i < nrA; i++)
1096 for (
unsigned int i = 0; i < nrB; i++)
1105 if (v.
data == NULL || v.
size() == 0) {
1125 if (v.
data == NULL || v.
size() == 0) {
1139 if (v.
data == NULL || v.
size() == 0) {
1143 double mean_value =
mean(v);
1144 double sum_squared_diff = 0.0;
1149 __m128d v_sub, v_mul, v_sum = _mm_setzero_pd();
1150 __m128d v_mean = _mm_set1_pd(mean_value);
1153 for (; i <= v.
getRows() - 4; i += 4) {
1154 v_sub = _mm_sub_pd(_mm_loadu_pd(v.
data + i), v_mean);
1155 v_mul = _mm_mul_pd(v_sub, v_sub);
1156 v_sum = _mm_add_pd(v_mul, v_sum);
1158 v_sub = _mm_sub_pd(_mm_loadu_pd(v.
data + i + 2), v_mean);
1159 v_mul = _mm_mul_pd(v_sub, v_sub);
1160 v_sum = _mm_add_pd(v_mul, v_sum);
1165 _mm_storeu_pd(res, v_sum);
1167 sum_squared_diff = res[0] + res[1];
1176 for (; i < v.
getRows(); i++) {
1177 sum_squared_diff += (v[i] - mean_value) * (v[i] - mean_value);
1180 double divisor = (double)v.
size();
1181 if (useBesselCorrection && v.
size() > 1) {
1182 divisor = divisor - 1;
1185 return std::sqrt(sum_squared_diff / divisor);
1210 M.
resize(3, 3,
false,
false);
1238 "Cannot compute the cross product between column "
1239 "vector with dimension %d and %d",
1318 if (
dsize != nrows * ncols) {
1323 M.
resize(nrows, ncols,
false,
false);
1325 for (
unsigned int j = 0; j < ncols; j++)
1326 for (
unsigned int i = 0; i < nrows; i++)
1327 M[i][j] =
data[j * nrows + i];
1391 int vpColVector::print(std::ostream &s,
unsigned int length,
char const *intro)
const
1393 typedef std::string::size_type size_type;
1398 std::vector<std::string> values(m * n);
1399 std::ostringstream oss;
1400 std::ostringstream ossFixed;
1401 std::ios_base::fmtflags original_flags = oss.flags();
1404 ossFixed.setf(std::ios::fixed, std::ios::floatfield);
1406 size_type maxBefore = 0;
1407 size_type maxAfter = 0;
1409 for (
unsigned int i = 0; i < m; ++i) {
1412 if (oss.str().find(
"e") != std::string::npos) {
1414 ossFixed << (*this)[i];
1415 oss.str(ossFixed.str());
1418 values[i] = oss.str();
1419 size_type thislen = values[i].size();
1420 size_type p = values[i].find(
'.');
1422 if (p == std::string::npos) {
1431 size_type totalLength = length;
1435 maxAfter = (std::min)(maxAfter, totalLength - maxBefore);
1444 s <<
"[" << m <<
"," << n <<
"]=\n";
1446 for (
unsigned int i = 0; i < m; i++) {
1448 size_type p = values[i].find(
'.');
1449 s.setf(std::ios::right, std::ios::adjustfield);
1450 s.width((std::streamsize)maxBefore);
1451 s << values[i].substr(0, p).c_str();
1454 s.setf(std::ios::left, std::ios::adjustfield);
1455 if (p != std::string::npos) {
1456 s.width((std::streamsize)maxAfter);
1457 s << values[i].substr(p, maxAfter).c_str();
1459 assert(maxAfter > 1);
1460 s.width((std::streamsize)maxAfter);
1470 s.flags(original_flags);
1472 return (
int)(maxBefore + maxAfter);
1487 __m128d v_sum1 = _mm_setzero_pd(), v_sum2 = _mm_setzero_pd(), v_sum;
1490 for (; i <=
rowNum - 4; i += 4) {
1491 v_sum1 = _mm_add_pd(_mm_loadu_pd(
data + i), v_sum1);
1492 v_sum2 = _mm_add_pd(_mm_loadu_pd(
data + i + 2), v_sum2);
1496 v_sum = _mm_add_pd(v_sum1, v_sum2);
1499 _mm_storeu_pd(res, v_sum);
1501 sum = res[0] + res[1];
1510 for (; i <
rowNum; i++) {
1525 double sum_square = 0.0;
1530 __m128d v_mul1, v_mul2;
1531 __m128d v_sum = _mm_setzero_pd();
1534 for (; i <=
rowNum - 4; i += 4) {
1535 v_mul1 = _mm_mul_pd(_mm_loadu_pd(
data + i), _mm_loadu_pd(
data + i));
1536 v_mul2 = _mm_mul_pd(_mm_loadu_pd(
data + i + 2), _mm_loadu_pd(
data + i + 2));
1538 v_sum = _mm_add_pd(v_mul1, v_sum);
1539 v_sum = _mm_add_pd(v_mul2, v_sum);
1544 _mm_storeu_pd(res, v_sum);
1546 sum_square = res[0] + res[1];
1556 for (; i <
rowNum; i++) {
1557 sum_square += (*this)[i] * (*this)[i];
1612 for (; i <=
dsize - 2; i += 2) {
1613 __m128d vout = _mm_mul_pd(_mm_loadu_pd(
data + i), _mm_loadu_pd(v.
data + i));
1614 _mm_storeu_pd(out.
data + i, vout);
1619 for (; i <
dsize; i++) {
1640 for (
unsigned int i = 0; i <
rowNum; i++) {
1641 double x = fabs((*
this)[i]);
1677 std::ostream &
vpColVector::cppPrint(std::ostream &os,
const std::string &matrixName,
bool octet)
const
1679 os <<
"vpColVector " << matrixName <<
" (" << this->
getRows() <<
"); " << std::endl;
1681 for (
unsigned int i = 0; i < this->
getRows(); ++i) {
1684 os << matrixName <<
"[" << i <<
"] = " << (*this)[i] <<
"; " << std::endl;
1686 for (
unsigned int k = 0; k <
sizeof(double); ++k) {
1687 os <<
"((unsigned char*)&(" << matrixName <<
"[" << i <<
"]) )[" << k <<
"] = 0x" << std::hex
1688 << (
unsigned int)((
unsigned char *)&((*this)[i]))[k] <<
"; " << std::endl;
1692 std::cout << std::endl;
1724 for (
unsigned int i = 0; i < this->
getRows(); ++i) {
1759 os <<
"([ " << std::endl;
1760 for (
unsigned int i = 0; i < this->
getRows(); ++i) {
1762 os << (*this)[i] <<
", ";
1763 os <<
"]," << std::endl;
1765 os <<
"])" << std::endl;
1808 for (
unsigned int i = 0; i < this->
getRows(); ++i) {
1809 os << (*this)[i] <<
", ";
1810 if (this->
getRows() != i + 1) {
1811 os <<
";" << std::endl;
1813 os <<
"]" << std::endl;
1819 #if defined(VISP_BUILD_DEPRECATED_FUNCTIONS)
1839 #endif // defined(VISP_BUILD_DEPRECATED_FUNCTIONS)