template < class ValueType > Vector < ValueType >
_mult(const Matrix< ValueType > & M, const Vector < ValueType > & b) {
Index cols = M.cols();
Index rows = M.rows();
Vector < ValueType > ret(rows, 0.0);
//ValueType tmpval = 0;
if (b.size() == cols){
for (Index i = 0; i < rows; ++i){
ret[i] = sum(M.mat_[i] * b);
// for (Index j = 0; j < cols; j++){
// ret[i] += M.mat_[i][j] * b[j];
// }
}
} else {
throwLengthError(1, WHERE_AM_I + " " + toStr(cols) + " != " + toStr(b.size()));
}
return ret;
}
performance curvy_step(const Matrix &F, Matrix &D){
performance data={0,0,0};
Matrix FD = F * D; data.multiplies++;
Matrix Eanti = FD - FD.transpose(); data.adds++;
Matrix Esymm = FD + FD.transpose(); data.adds++;
Matrix X = Eigen::MatrixXd::Zero(F.cols(), F.rows());
conjugategradient(F,X,D,Esymm,Eanti,data);
double scale = 0.001/X.lpNorm<Eigen::Infinity>();
X = scale * X;
std::size_t rotation_order = 2;
BCH_rotate(D, X, rotation_order, data);
Purify(D, data);
return data;
}
template<typename Scalar> void
initSPD(double density,
Matrix<Scalar,Dynamic,Dynamic>& refMat,
SparseMatrix<Scalar>& sparseMat)
{
Matrix<Scalar,Dynamic,Dynamic> aux(refMat.rows(),refMat.cols());
initSparse(density,refMat,sparseMat);
refMat = refMat * refMat.adjoint();
for (int k=0; k<2; ++k)
{
initSparse(density,aux,sparseMat,ForceNonZeroDiag);
refMat += aux * aux.adjoint();
}
sparseMat.setZero();
for (int j=0 ; j<sparseMat.cols(); ++j)
for (int i=j ; i<sparseMat.rows(); ++i)
if (refMat(i,j)!=Scalar(0))
sparseMat.insert(i,j) = refMat(i,j);
sparseMat.finalize();
}
void Cholesky<Matrix>::doCompute( const Matrix& mat )
{
if (!validation(mat))
{
std::cerr<<"Warning in cholesky solver: the input matrix is not symmetric. Please confirm that!"<<std::endl;
return;
}
clear();
__a = new scalar[mat.size()];
this->setRowNum(mat.rows());
this->setColNum(mat.cols());
this->setLDA(mat.lda());
blas::copt_blas_copy(mat.size(),mat.dataPtr(),1,__a,1);
copt_lapack_potrf('U',this->rowNum(),__a,this->lda(),&__info);
if( __info != 0 )
{
std::cout<<__info<<std::endl;
std::cerr<<"Warning in Cholesky solver: something computation is wrong!"<<std::endl;
}
}
TEST_P (RandomWalkerTest, GetPotentials)
{
Matrix p;
std::map<Color, size_t> map;
pcl::segmentation::randomWalker (g.graph,
boost::get (boost::edge_weight, g.graph),
boost::get (boost::vertex_color, g.graph),
p,
map);
ASSERT_EQ (g.size, p.rows ());
ASSERT_EQ (g.colors.size (), p.cols ());
ASSERT_EQ (g.colors.size (), map.size ());
for (std::set<Color>::iterator it = g.colors.begin (); it != g.colors.end (); ++it)
for (size_t i = 0; i < g.size; ++i)
if (g.potentials.count (*it))
EXPECT_NEAR (g.potentials[*it] (i), p (i, map[*it]), 0.01);
}
void Matrix <T> ::row ( const Matrix <T> &M, const unsigned int r )
{
//Matrix dimemension invalid, throw exception
if ( r > rows_count )
{
throw ErrorIndexOutOfBound ( "ErrorIndexOutOfBound: ", " row index exceeds rows_count. " );
}
//Matrix dimemension invalid, throw exception
if ( M.cols() != columns_count )
{
throw ErrorMathMatrixDifferentSize <Matrix <T> > ( "ErrorMathMatrixDifferentSize: ", " invalid dimension of the matrices (columns_count count). ", *this, M );
}
//Copy row
for ( unsigned int i = 0; i < columns_count; i++ )
{
items[r][i] = M ( 0, i );
}
}
Scalar minDistanceSquared(
const Gaussian<Scalar, NDims>& model,
const Matrix<Scalar, NDims, Dynamic>& bag,
int* index
) {
Scalar minDistance = numeric_limits<Scalar>::infinity();
int minIndex = -1;
for (int p = 0; p < bag.cols(); p++) {
Matrix<Scalar, NDims, 1> point(bag.col(p));
Scalar distance = model.distanceSquared(point);
if (distance < minDistance) {
minDistance = distance;
minIndex = p;
}
}
if (index) {
*index = minIndex;
}
return minDistance;
}
void
getPotentials (Matrix& potentials, std::map<Color, size_t>& color_to_column_map)
{
using namespace boost;
potentials = Matrix::Zero (num_vertices (g_), colors_.size ());
// Copy over rows from X
for (int i = 0; i < X.rows (); ++i)
potentials.row (L_vertex_bimap.left.at (i)).head (X.cols ()) = X.row (i);
// In rows that correspond to seeds put ones in proper columns
for (size_t i = 0; i < seeds_.size (); ++i)
{
VertexDescriptor v = seeds_[i];
insertInBimap (B_color_bimap, color_map_[v]);
potentials (seeds_[i], B_color_bimap.right.at (color_map_[seeds_[i]])) = 1;
}
// Fill in a map that associates colors with columns in potentials matrix
color_to_column_map.clear ();
for (int i = 0; i < potentials.cols (); ++i)
color_to_column_map[B_color_bimap.left.at (i)] = i;
}
Matrix yarp::math::SE3inv(const Matrix &H)
{
yAssert((H.rows()==4) && (H.cols()==4));
Vector p(4);
p[0]=H(0,3);
p[1]=H(1,3);
p[2]=H(2,3);
p[3]=1.0;
Matrix invH=H.transposed();
p=invH*p;
invH(0,3)=-p[0];
invH(1,3)=-p[1];
invH(2,3)=-p[2];
invH(3,0)=invH(3,1)=invH(3,2)=0.0;
return invH;
}
Vector yarp::math::dcm2axis(const Matrix &R)
{
yAssert((R.rows()>=3) && (R.cols()>=3));
Vector v(4);
v[0]=R(2,1)-R(1,2);
v[1]=R(0,2)-R(2,0);
v[2]=R(1,0)-R(0,1);
v[3]=0.0;
double r=yarp::math::norm(v);
double theta=atan2(0.5*r,0.5*(R(0,0)+R(1,1)+R(2,2)-1));
if (r<1e-9)
{
// if we enter here, then
// R is symmetric; this can
// happen only if the rotation
// angle is 0 (R=I) or 180 degrees
Matrix A=R.submatrix(0,2,0,2);
Matrix U(3,3), V(3,3);
Vector S(3);
// A=I+sin(theta)*S+(1-cos(theta))*S^2
// where S is the skew matrix.
// Given a point x, A*x is the rotated one,
// hence if Ax=x then x belongs to the rotation
// axis. We have therefore to find the kernel of
// the linear application (A-I).
SVD(A-eye(3,3),U,S,V);
v[0]=V(0,2);
v[1]=V(1,2);
v[2]=V(2,2);
r=yarp::math::norm(v);
}
v=(1.0/r)*v;
v[3]=theta;
return v;
}
veDouble rowStd(const Matrix& M, const veDouble& avg) {
uint m = M.rows();
uint n = M.cols();
if (n < 3)
return(veDouble(m, 0.0));
veDouble stddev;
for (uint j=0; j<m; ++j) {
double x = 0.0;
for (uint i=0; i<n; ++i) {
double d = M(j, i) - avg[j];
x += d*d;
}
stddev.push_back(sqrt(x/(n-1.0)));
}
return(stddev);
}
void Matrix<T>::submatrix(int sr, int sc, Matrix<T> &a)
{
int rwz,coz,i,j;
if ( this->rows() % a.rows() != 0 || this->cols() % a.cols() != 0 || this->rows() < a.rows() || this->cols() < a.cols() )
{
#ifdef USE_EXCEPTION
throw WrongSize2D(this->rows(),this->cols(),a.rows(),a.cols()) ;
#else
Error error("Matrix<T>::submatrix");
error << "Matrix and submatrix incommensurate" ;
error.fatal() ;
#endif
}
if ( sr >= this->rows()/a.rows() || sr < 0 || sc >= this->cols()/a.cols() || sc < 0 )
{
#ifdef USE_EXCEPTION
throw OutOfBound2D(sr,sc,0,this->rows()/a.rows()-1,0,this->cols()/a.cols()-1) ;
#else
Error error("Matrix<T>::submatrix");
error << "Submatrix location out of bounds.\nrowblock " << sr << ", " << rows()/a.rows() << " colblock " << sc << ", " << a.cols() << endl ;
error.fatal() ;
#endif
}
rwz = sr*a.rows();
coz = sc*a.cols();
#ifdef COLUMN_ORDER
for ( i = a.rows()-1; i >= 0; --i )
for(j=a.cols()-1;j>=0;--j)
this->elem(i+rwz,j+coz) = a(i,j) ;
#else
T *ptr, *aptr ;
aptr = a.m - 1;
for ( i = a.rows()-1; i >= 0; --i )
{
ptr = &m[(i+rwz)*cols()+coz]-1 ;
for ( j = a.cols(); j > 0; --j)
*(++ptr) = *(++aptr) ;
}
#endif
}
Matrix gramschmidt( const Matrix & A ) {
Matrix Q = A;
// First vector just gets normalized
Q.col(0).normalize();
for(unsigned int j = 1; j < A.cols(); ++j) {
// Replace inner loop over each previous vector in Q with fast matrix-vector multiplication
Q.col(j) -= Q.leftCols(j) * (Q.leftCols(j).transpose() * A.col(j));
// Normalize vector if possible (othw. means colums of A almsost lin. dep.
if( Q.col(j).norm() <= 10e-14 * A.col(j).norm() ) {
std::cerr << "Gram-Schmidt failed because A has lin. dep columns. Bye." << std::endl;
break;
} else {
Q.col(j).normalize();
}
}
return Q;
}
// check *above the diagonal* for non-zero entries
boost::optional<Vector> checkIfDiagonal(const Matrix M) {
size_t m = M.rows(), n = M.cols();
// check all non-diagonal entries
bool full = false;
size_t i, j;
for (i = 0; i < m; i++)
if (!full)
for (j = i + 1; j < n; j++)
if (fabs(M(i, j)) > 1e-9) {
full = true;
break;
}
if (full) {
return boost::none;
} else {
Vector diagonal(n);
for (j = 0; j < n; j++)
diagonal(j) = M(j, j);
return diagonal;
}
}
typename PointMesher<T>::Matrix PointMesher<T>::ITMLocalMesher::delaunay2D(const Matrix matrixIn) const
{
typedef CGAL::Exact_predicates_inexact_constructions_kernel K;
typedef CGAL::Triangulation_vertex_base_with_info_2<int, K> Vb;
typedef CGAL::Triangulation_face_base_2<K> Fb;
typedef CGAL::Triangulation_data_structure_2<Vb, Fb> Tds;
typedef CGAL::Delaunay_triangulation_2<K, Tds> DelaunayTri;
typedef DelaunayTri::Finite_faces_iterator Finite_faces_iterator;
typedef DelaunayTri::Vertex_handle Vertex_handle;
typedef DelaunayTri::Point Point;
// Compute triangulation
DelaunayTri dt;
for (int i = 0; i < matrixIn.cols(); i++)
{
// Add point
Point p = Point(matrixIn(0, i), matrixIn(1, i));
dt.push_back(Point(p));
// Add index
Vertex_handle vh = dt.push_back(p);
vh->info() = i;
}
// Iterate through faces
Matrix matrixOut(3, dt.number_of_faces());
int itCol = 0;
DelaunayTri::Finite_faces_iterator it;
for (it = dt.finite_faces_begin(); it != dt.finite_faces_end(); it++)
{
for (int k = 0; k < matrixOut.rows(); k++)
{
// Add index of triangle vertex to list
matrixOut(k, itCol) = it->vertex(k)->info();
}
itCol++;
}
return matrixOut;
}
Matrix<ValueType> evaluateLocalBasis(const Space<double>& space, const Entity<0>& element,
const Matrix<double>& local, const Vector<ValueType>& localCoefficients)
{
if (local.rows() != space.grid()->dim())
throw std::invalid_argument("evaluate(): points in 'local' have an "
"invalid number of coordinates");
const int nComponents = space.codomainDimension();
Matrix<ValueType> values(nComponents, local.cols());
values.setZero();
// Find out which basis data need to be calculated
size_t basisDeps = 0, geomDeps = 0;
// Find out which geometrical data need to be calculated,
const Fiber::CollectionOfShapesetTransformations<double>
&transformations = space.basisFunctionValue();
transformations.addDependencies(basisDeps, geomDeps);
// Get basis data
const Fiber::Shapeset<double> &shapeset =
space.shapeset(element);
Fiber::BasisData<double> basisData;
shapeset.evaluate(basisDeps, local, ALL_DOFS, basisData);
// Get geometrical data
Fiber::GeometricalData<double> geomData;
element.geometry().getData(geomDeps, local, geomData);
// Get shape function values
Fiber::CollectionOf3dArrays<double> functionValues;
transformations.evaluate(basisData, geomData, functionValues);
// Calculate grid function values
for (size_t p = 0; p < functionValues[0].extent(2); ++p)
for (size_t f = 0; f < functionValues[0].extent(1); ++f)
for (size_t dim = 0; dim < functionValues[0].extent(0); ++dim)
values(dim, p) += functionValues[0](dim, f, p) * localCoefficients(f);
return values;
}
bool CalibModule::getGazeParams(const string &eye, const string &type, Matrix &M)
{
if (((eye!="left") && (eye!="right")) ||
((type!="intrinsics") && (type!="extrinsics")))
return false;
Bottle info;
igaze->getInfo(info);
if (Bottle *pB=info.find(("camera_"+type+"_"+eye).c_str()).asList())
{
M.resize((type=="intrinsics")?3:4,4);
int cnt=0;
for (int r=0; r<M.rows(); r++)
for (int c=0; c<M.cols(); c++)
M(r,c)=pB->get(cnt++).asDouble();
return true;
}
else
return false;
}
// -----------------------------------------------------------------------------------
//------------------------------------------------------------------------------------
// Compute the azimuth and nadir angle, in the satellite body frame,
// of receiver Position RX as seen at the satellite Position SV. The nadir angle
// is measured from the Z axis, which points to Earth center, and azimuth is
// measured from the X axis.
// @param Position SV Satellite position
// @param Position RX Receiver position
// @param Matrix<double> Rot Rotation matrix (3,3), output of SatelliteAttitude
// @param double nadir Output nadir angle in degrees
// @param double azimuth Output azimuth angle in degrees
void SatelliteNadirAzimuthAngles(const Position& SV,
const Position& RX,
const Matrix<double>& Rot,
double& nadir,
double& azimuth)
throw(Exception)
{
try {
if(Rot.rows() != 3 || Rot.cols() != 3) {
Exception e("Rotation matrix invalid");
GPSTK_THROW(e);
}
double d;
Position RmS;
// RmS points from satellite to receiver
RmS = RX - SV;
RmS.transformTo(Position::Cartesian);
d = RmS.mag();
if(d == 0.0) {
Exception e("Satellite and Receiver Positions identical");
GPSTK_THROW(e);
}
RmS = (1.0/d) * RmS;
Vector<double> XYZ(3),Body(3);
XYZ(0) = RmS.X();
XYZ(1) = RmS.Y();
XYZ(2) = RmS.Z();
Body = Rot * XYZ;
nadir = ::acos(Body(2)) * RAD_TO_DEG;
azimuth = ::atan2(Body(1),Body(0)) * RAD_TO_DEG;
if(azimuth < 0.0) azimuth += 360.;
}
catch(Exception& e) { GPSTK_RETHROW(e); }
catch(exception& e) {Exception E("std except: "+string(e.what())); GPSTK_THROW(E);}
catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}
Matrix<double> Wscorerate_cox(unsigned Var,
const Matrix<double> &X,
const Matrix<double> &schoen,
const Matrix<double> &RR,
const Matrix<double> &E,
const Matrix<double> &S_0,
const Matrix<double> &cumhaz,
const Matrix<double> &beta_iid,
const Matrix<double> &It,
const Matrix<unsigned> &index_dtimes,
const Matrix<double> &time) {
Matrix<double> dtimes = chrows(time, index_dtimes);
unsigned n=X.rows(); unsigned p=X.cols(); unsigned nd=dtimes.size();
Matrix<double> schoendN(n,p); // Initialized as zero
for (unsigned i=0; i<nd; i++)
schoendN(index_dtimes[i],_) = schoen(i,_);
Matrix<double> dMscorerate(n,nd); // component 'Var' of score
for (unsigned i=0; i<nd; i++) {
Matrix<double> risk = (time>=dtimes[i]);
dMscorerate(_,i) = -risk%RR%(X(_,Var)-E(i,Var))/S_0[i];
dMscorerate(index_dtimes[i],i) = schoen(i,Var)+dMscorerate(index_dtimes[i],i);
}
Matrix<double> Mscorerate = (cumsum(t(dMscorerate)));
Matrix<double> Wscorerate(n,nd);
for (unsigned i=0; i<n; i++) {
Matrix<double> betaiidrow = t(beta_iid(i,_));
for (unsigned j=0; j<nd; j++) {
Matrix<double> Itd = It(j,_); Itd.resize(p,p);
Wscorerate(i,j) = Mscorerate(j,i) - (Itd*betaiidrow)[Var];
}
}
return(Wscorerate);
}
void RigidBodyTree::accumulateContactJacobian(
const KinematicsCache<Scalar> &cache, const int bodyInd,
Matrix3Xd const &bodyPoints, std::vector<size_t> const &cindA,
std::vector<size_t> const &cindB,
Matrix<Scalar, Eigen::Dynamic, Eigen::Dynamic> &J) const {
const auto nq = J.cols();
const size_t numCA = cindA.size();
const size_t numCB = cindB.size();
const size_t offset = 3 * numCA;
auto J_tmp = transformPointsJacobian(cache, bodyPoints, bodyInd, 0, true);
// add contributions from points in xA
for (size_t x = 0; x < numCA; x++) {
J.block(3 * cindA[x], 0, 3, nq) += J_tmp.block(3 * x, 0, 3, nq);
}
// subtract contributions from points in xB
for (size_t x = 0; x < numCB; x++) {
J.block(3 * cindB[x], 0, 3, nq) -= J_tmp.block(offset + 3 * x, 0, 3, nq);
}
}
void checkScaleMinMax() {
Printf("Check scale Min-Max+++++++++++++++++++++++++++\n");
Matrix m(3, {1, 5, 9,
2, 3, 39,
7, 15, 22});
print(m);
double min = -1;
double max = 1;
Matrix testM = m;
scaleMinMax(min, max, testM);
print(testM);
double tMin = 1000, tMax = -1000;
for (int i = 0 ; i < testM.rows(); i++) {
for (int j = 0; j < testM.cols(); j++) {
if (tMax < testM(i, j)) {
tMax = testM(i, j);
}
if (tMin > testM(i, j)) {
tMin = testM(i, j);
}
}
}
unitpp::assert_true(spf("Expected matrix min should be greater than: %f, but was: %f", min, tMin), tMin >= min);
unitpp::assert_true(spf("Expected matrix max should be less than: %f, but was: %f", max, tMax), tMax <= max);
// check scale by indices
VI indices = {0, 2};
Matrix testM2 = m;
scaleMinMax(min, max, indices, testM2);
print(testM2);
Matrix checkM(3, {-1, 5, -1,
-0.666666, 3, 1,
1, 15, -0.133333});
unitpp::assert_true("Scale by min/max with column indices failed", testM2.similar(checkM, 0.000001));
}
static lu_pair<scalar> lu_decomposition(const Matrix<scalar>& A)
{
assert(A.rows() == A.cols());
auto n = A.rows();
Matrix<scalar> L = Matrix<scalar>::Zero(n, n);
Matrix<scalar> U = Matrix<scalar>::Zero(n, n);
if (A.rows() == 1) {
L(0, 0) = 1.0;
U(0, 0) = A(0,0);
} else {
// first row of U is first row of A
auto U12 = A.block(0, 1, 1, n-1);
// first column of L is first column of A / a11
auto L21 = A.block(1, 0, n-1, 1) / A(0, 0);
// remove first row and column and recurse
auto A22 = A.block(1, 1, n-1, n-1);
Matrix<scalar> tmp = A22 - L21 * U12;
auto LU22 = lu_decomposition(tmp);
L(0, 0) = 1.0;
U(0, 0) = A(0, 0);
L.block(1, 0, n-1, 1) = L21;
U.block(0, 1, 1, n-1) = U12;
L.block(1, 1, n-1, n-1) = get<0>(LU22);
U.block(1, 1, n-1, n-1) = get<1>(LU22);
}
return lu_pair<scalar>(L, U);
}
void Matrix <T> ::submat ( const Matrix <T> & M, const unsigned int row, const unsigned int col )
{
const unsigned int m = M.rows(), n = M.cols();
if ( m + row > rows_count )
{
throw ErrorIndexOutOfBound ( "ErrorIndexOutOfBound: a submatrix does not fit at the specified row position, ", "can not append a submatrix to the matrix. " );
}
if ( n + col > columns_count )
{
throw ErrorIndexOutOfBound ( "ErrorIndexOutOfBound: a submatrix does not fit at the specified col position, ", "can not append a submatrix to the matrix." );
}
//Copy submatrix
for ( unsigned int i = 0; i < m; i++ )
{
for ( unsigned int j = 0; j < n; j++ )
{
items [i + row][j + col] = M ( i, j );
}
}
}
Vector yarp::math::dcm2euler(const Matrix &R)
{
yAssert((R.rows()>=3) && (R.cols()>=3));
Vector v(3);
bool singularity=false;
if (R(2,2)<1.0)
{
if (R(2,2)>-1.0)
{
v[0]=atan2(R(1,2),R(0,2));
v[1]=acos(R(2,2));
v[2]=atan2(R(2,1),-R(2,0));
}
else
{
// Not a unique solution: gamma-alpha=atan2(R10,R11)
singularity=true;
v[0]=-atan2(R(1,0),R(1,1));
v[1]=M_PI;
v[2]=0.0;
}
}
else
{
// Not a unique solution: gamma+alpha=atan2(R10,R11)
singularity=true;
v[0]=atan2(R(1,0),R(1,1));
v[1]=0.0;
v[2]=0.0;
}
if (singularity)
yWarning("dcm2euler() in singularity: choosing one solution among multiple");
return v;
}
Vector yarp::math::dcm2rpy(const Matrix &R)
{
yAssert((R.rows()>=3) && (R.cols()>=3));
Vector v(3);
bool singularity=false;
if (R(2,0)<1.0)
{
if (R(2,0)>-1.0)
{
v[0]=atan2(R(2,1),R(2,2));
v[1]=asin(-R(2,0));
v[2]=atan2(R(1,0),R(0,0));
}
else
{
// Not a unique solution: psi-phi=atan2(-R12,R11)
singularity = true;
v[0]=0.0;
v[1]=M_PI/2.0;
v[2]=-atan2(-R(1,2),R(1,1));
}
}
else
{
// Not a unique solution: psi+phi=atan2(-R12,R11)
singularity = true;
v[0]=0.0;
v[1]=-M_PI/2.0;
v[2]=atan2(-R(1,2),R(1,1));
}
if (singularity)
yWarning("dcm2rpy() in singularity: choosing one solution among multiple");
return v;
}
BenchResult doBenchFLANN(const Matrix& d, const Matrix& q, const Index K, const int itCount)
{
BenchResult result;
const int dimCount(d.rows());
const int dPtCount(d.cols());
const int qPtCount(itCount);
flann::Matrix<T> dataset(new T[dPtCount*dimCount], dPtCount, dimCount);
for (int point = 0; point < dPtCount; ++point)
for (int dim = 0; dim < dimCount; ++dim)
dataset[point][dim] = d(dim, point);
flann::Matrix<T> query(new T[qPtCount*dimCount], qPtCount, dimCount);
for (int point = 0; point < qPtCount; ++point)
for (int dim = 0; dim < dimCount; ++dim)
query[point][dim] = q(dim, point);
flann::Matrix<int> indices(new int[query.rows*K], query.rows, K);
flann::Matrix<float> dists(new float[query.rows*K], query.rows, K);
// construct an randomized kd-tree index using 4 kd-trees
boost::timer t;
flann::Index<T> index(dataset, flann::KDTreeIndexParams(4) /*flann::AutotunedIndexParams(0.9)*/); // exact search
index.buildIndex();
result.creationDuration = t.elapsed();
t.restart();
// do a knn search, using 128 checks
index.knnSearch(query, indices, dists, int(K), flann::SearchParams(128)); // last parameter ignored because of autotuned
result.executionDuration = t.elapsed();
dataset.free();
query.free();
indices.free();
dists.free();
return result;
}
/*
* "Chase the bulge" down J.
* As a result of this method, J will remain bidiagonal,
* but off-diagonals will be reduced, and U & V accumulate the rotations.
* p and q are matrix dimensions of the block representation of J.
*/
static void chase(Matrix& J, Matrix& U, Matrix& V, int p, int q) {
const int m = J.rows(), n = J.cols();
const int N = n - q;
double w, cs, sn;
Matrix S(m), T(n);
//Compute Wilkinson shift and T2
w = computeWilkinson(sq(J.at(N - 2, N - 2)) + sq(J.at(N - 2, N - 1)),
J.at(N - 2, N - 1) * J.at(N - 1, N - 1),
sq(J.at(N - 1, N - 1)));
rot(an2(J, p) - w, an(J, p) * bn(J, p + 1), cs, sn);
T = givensCS(n, p, p + 1, cs, sn);
J = J * T;
V = V * T;
//Perform Givens rotation to chase the values down the off-bidiagonals
for (int i = p; i < N - 1; i++) {
if (i > p) {
//Compute Ti, annihilating values above superdiagonal values
rot(J.at(i - 1, i), J.at(i - 1, i + 1), cs, sn);
T = givensCS(n, i, i + 1, cs, sn);
J = J * T;
V = V * T;
}
//Compute Si^T, annihilating subdiagonal values
rot(J.at(i, i), J.at(i + 1, i), cs, sn);
S = givensCS(m, i, i + 1, cs, -sn);
J = S * J;
U = U * S.trans();
}
}
int main(int argc, char *argv[]) {
std::cout << "Starting a serial matrix multiplication. \n " << std::endl;
// Read in the input: N
if ( argv[1]== NULL ){ // check input is supplied
std::cout << "ERROR: The program must be executed in the following way \n\n \t \"./serial.exe N \" \n\n where N is an integer. \n \n " << std::endl;
return 1;
}
int N = atoi(argv[1]); // The dimensions of the matrices MUST be specified at runtime.
std::cout << "The matrices are: " << N<<"x"<<N<< std::endl;
// for simplicity, all matricies will be NxN
int numberOfRowsA = N; int numberOfColsA = N; int numberOfRowsB = N; int numberOfColsB = N;
//Declare matrices: Matix class is a 2D vetor
Matrix<double> A = Matrix<double>(numberOfRowsA, numberOfColsA);
Matrix<double> B = Matrix<double>(numberOfRowsB, numberOfColsB);
Matrix<double> C = Matrix<double>(numberOfRowsA, numberOfColsB);
FillMatricesRandomly(A, B); /* Excluded from timing!!! */
struct timespec start, end;
clock_gettime(CLOCK_MONOTONIC, &start); // start timing
// Matrix Multiplication
for (int i = 0; i < A.rows(); i++) {//iterate through rows of A
for (int j = 0; j < B.cols(); j++) {//iterate through columns of B
for (int k = 0; k < B.rows(); k++) {//iterate through rows of B
C(i,j) += (A(i,k) * B(k,j));
}
}
}
clock_gettime(CLOCK_MONOTONIC, &end); // end timing
double matrixCalculationTime = (end.tv_sec - start.tv_sec) + (end.tv_nsec - start.tv_nsec)*1e-9;
std::cout << "\nTotal multplication time = " << matrixCalculationTime << std::endl;
// PrintMatrices(A, B, C);
return 0;
}
std::shared_ptr<Matrix> NearestNeighborNoiseEstimation(const Matrix &data) {
// TODO: Outlier exclusion
Index d = data.cols();
Index n = data.rows();
std::shared_ptr<Matrix> result = std::make_shared<Matrix>(d, d);
Matrix noise_values(n, d);
auto l2_dist = L2Distance(data, data, true);
// Find the second smallest distance (first is 0, and should be forced, so it is smallest non-zero), so no need to fully sort the matrix
for (Index i = 0; i < n; ++i) {
Index smallest_non_zero_index = 0;
Scalar smallest_non_zero_value = (*l2_dist)(i, 0);
if (i == 0) {
smallest_non_zero_value = std::numeric_limits<Scalar>::max();
}
for (Index j = 1; j < n; ++j) {
if (j == i) continue;
if ((*l2_dist)(i, j) < smallest_non_zero_value) {
smallest_non_zero_index = j;
smallest_non_zero_value = (*l2_dist)(i, j);
}
}
for (Index k = 0; k < d; ++k) {
noise_values(i, k) = data(i, k) - data(smallest_non_zero_index, k);
}
}
gsl_blas_dgemm(CblasTrans, CblasNoTrans, 1.0, noise_values.m_, noise_values.m_, 0.0, result->m_);
return result;
}
YARP_END_PACK
/// network stuff
#include <yarp/os/NetInt32.h>
bool yarp::sig::removeCols(const Matrix &in, Matrix &out, int first_col, int how_many)
{
int nrows = in.rows();
int ncols = in.cols();
Matrix ret(nrows, ncols-how_many);
for(int r=0; r<nrows; r++)
for(int c_in=0,c_out=0;c_in<ncols; c_in++)
{
if (c_in==first_col)
{
c_in=c_in+(how_many-1);
continue;
}
ret[r][c_out]=(in)[r][c_in];
c_out++;
}
out=ret;
return true;
}
