bool
MappedImage::merge( const boost::numeric::ublas::matrix<uint16_t>& frame, unsigned low, unsigned high )
{
if ( frame.size1() != impl_->data_.size1() || frame.size2() != impl_->data_.size2() ) {
// dimension mismatch
impl_->data_.resize( frame.size1(), frame.size2() );
impl_->data_.clear();
impl_->mergeCount_ = 0;
}
for ( size_t i = 0; i < frame.size1(); ++i ) {
for ( size_t j = 0; j < frame.size2(); ++j ) {
if ( auto tof = frame(i, j) ) { // has hit
if ( low <= tof && tof <= high ) {
impl_->data_( i, j ) += 1.0;
impl_->z_ = std::max( impl_->z_, impl_->data_( i, j ) );
}
}
}
}
impl_->mergeCount_++;
return true;
}
double det_chol(const ublas::matrix<double,F,A> &m)
// Compute determinant of Cholesky matrix.
{
assert(m.size1() == m.size2());
double d = 1.;
for (size_t i=0; i < m.size1(); ++i)
d *= m(i,i);
return d;
}
double diameter(const ublas::matrix<double>& D)
{
double total = 0;
for(int i=0;i<D.size1();i++)
for(int j=0;j<i;j++)
total += D(i,j);
int N = D.size1() * (D.size1() - 1) /2;
return total/N;
}
double determinant( bnu::matrix<double>& m ) {
bnu::permutation_matrix<std ::size_t> pm(m.size1());
double det = 1.0;
if( bnu::lu_factorize(m,pm) ) {
det = 0.0;
} else {
for(int i = 0; i < m.size1(); i++)
det *= m(i,i); // multiply by elements on diagonal
det = det * determinant_sign( pm );
}
return det;
}
bool is_symmetric(const ublas::matrix<double, F, A> &m)
{
if (m.size1() != m.size2())
return false;
for (size_t i = 0; i < m.size1(); ++i)
for (size_t j = i+1; j < m.size2(); ++j)
if (m(i,j) != m(j,i))
return false;
return true;
}
cv::Mat Bridge::createImgMap(const boost::numeric::ublas::matrix<STATE> &groundMap) {
cv::Mat ground(groundMap.size1(), groundMap.size2(), CV_8UC3, cv::Scalar(0, 0, 0));
for (unsigned int i = 0; i < groundMap.size1(); ++i) {
for (unsigned int j = 0; j < groundMap.size2(); ++j) {
if (groundMap(i, j) == 0) {
ground.at<cv::Vec3b>(i, j) = cv::Vec3b(0, 0, 0);
} else {
ground.at<cv::Vec3b>(i, j) = cv::Vec3b(255, 255, 255);
}
}
}
return ground;
}
void pprint(const boost::numeric::ublas::matrix<double>& m) {
cout << "[";
for( uint r=0; r < m.size1(); r++) {
for( uint c=0; c < m.size2(); c++) {
cout << m(r,c);
if(c == m.size2()-1 && r != m.size1()-1)
cout << "\n ";
else if(c != m.size2()-1)
cout << " , ";
}
}
cout << "]\n";
}
bool equal(boost::numeric::ublas::matrix<E> lhs, boost::numeric::ublas::matrix<E> rhs)
{
if (&lhs == &rhs) { return true; }
if (lhs.size1() != rhs.size1()) { return false; }
if (lhs.size2() != rhs.size2()) { return false; }
typename boost::numeric::ublas::matrix<E>::iterator1 l(lhs.begin1());
typename boost::numeric::ublas::matrix<E>::iterator1 r(rhs.begin1());
while (l != lhs.end1()) {
if (*l != *r) { return false; }
++l;
++r;
}
return true;
}
FixedMatrix( const boost::numeric::ublas::matrix<TYPE> &boost_matrix ) throw ( std::logic_error & ) {
if( boost_matrix.size1() == ROWS && boost_matrix.size2() == COLS ) {
for( size_t m = 0; m < ROWS; m++ ) {
for( size_t n = 0; n < COLS; n++ ) {
this->elem( n, m ) = boost_matrix( m, n );
}
}
} else {
LOG( Runtime, error ) << "The size of the boost matrix ("
<< boost_matrix.size1() << ", " << boost_matrix.size2()
<< ") does not coincide with the size of the isis matrix (" << ROWS << ", " << COLS << ").";
throw( std::logic_error( "Size mismatch" ) );
}
};
int SAFForwardingTable::determineRowOfFace(int face_id, boost::numeric::ublas::matrix<double> tab, std::vector<int> faces)
{
// check if table fits to faces
if(tab.size1 () != faces.size ())
{
fprintf(stderr, "Error the number of faces dont correspond to the table!\n");
return FACE_NOT_FOUND;
}
if(std::find(faces.begin (), faces.end (), face_id) == faces.end ())
{
fprintf(stderr, "Face Not Found!!!\n");
return FACE_NOT_FOUND;
}
//determine row of face
int faceRow = FACE_NOT_FOUND;
std::sort(faces.begin(), faces.end());//order
int rowCounter = 0;
for(std::vector<int>::iterator i = faces.begin (); i != faces.end() ; ++i)
{
//fprintf(stderr, "*i=%d ; face_id=%d\n",*i,face_id);
if(*i == face_id)
{
faceRow = rowCounter;
break;
}
rowCounter++;
}
return faceRow;
}
boost::array<WorkingType, ValDim>
interpolate(const boost::array<WorkingType, PosDim> &position) const
{
boost::array<WorkingType, ValDim> result;
// Init result
for(int j(0); j < ValDim; ++j)
result[j] = 0;
unsigned int i(0);
for(; i < Wa.size1() - (PosDim+1); ++i)
{
for(int j(0); j < ValDim; ++j)
result[j] += Wa(i,j) * radialbasis<WorkingType, WorkingType, PosDim>(refPositions[i], position);
}
for(int j(0); j < ValDim; ++j)
result[j] += Wa(i,j);
++i;
for(int k(0); k < PosDim; ++k, ++i)
{
for(int j(0); j < ValDim; ++j)
result[j] += Wa(i,j) * position[k];
}
return result;
}
void force_symmetry(ublas::matrix<double, F, A> &m, const bool upperToLower=true)
// Make matrix symmetric by copying upper-to-lower (default) or lower-to-upper.
{
if (m.size1() != m.size2())
throw LogicalError(ERROR_INFO("Matrix is not square"));
if (upperToLower) {
for (size_t i = 0; i < m.size1(); ++i)
for (size_t j = i+1; j < m.size2(); ++j)
m(j,i) = m(i,j);
}
else {
for (size_t i = 0; i < m.size1(); ++i)
for (size_t j = i+1; j < m.size2(); ++j)
m(i,j) = m(j,i);
}
}
void parallel_jacob_musictest(boost::numeric::ublas::matrix<float> M, std::string isWriteToConsole, std::ofstream fp_outs[1], int i) {
int iter;
auto begin = std::chrono::high_resolution_clock::now();
auto end = std::chrono::high_resolution_clock::now();
double duration = 0;
matrix* A = 0;
init_matrix(&A, M.size1());
make_matrix(*A, M);
std::vector<float> e;
// BEGIN TEST
begin = std::chrono::high_resolution_clock::now();
find_eigenvalues_parallel_jacob_music(A, e, iter);
end = std::chrono::high_resolution_clock::now();
// END TEST
std::sort(e.begin(), e.end());
duration = std::chrono::duration_cast<std::chrono::nanoseconds>(end - begin).count() / 1000000.0;
boost::numeric::ublas::vector<float> eigs(M.size1());
for (size_t i = 0; i < e.size(); i++)
{
eigs(i) = e[i];
}
// INFO
if (isWriteToConsole == "true") {
std::string eig = "[";
eig += std::to_string(M.size1());
eig += "](";
for (int i = 0; i < M.size1() - 1; i++)
{
eig += std::to_string(e[i]);
eig += ",";
}
eig += std::to_string(e[M.size1() - 1]);
eig += ")";
writeToAllStreams((boost::format("#%1%: \n") % i).str(), fp_outs);
writeToAllStreams((boost::format("Name: %1% \nEigenvalues: %2% \nElapsed(ms): %3% \nIter: %4%")
% "parallel_jacob_music"% eig%duration%iter).str(), fp_outs);
writeToAllStreams("============================", fp_outs);
}
}
void fill_boost_matrix_from_munkres_matrix (boost::numeric::ublas::matrix <T> & boost_matrix, const Matrix <T> & matrix)
{
const int dimention = std::min (boost_matrix.size1 (), boost_matrix.size2 () );
for (int i = 0; i < dimention; ++i) {
for (int j = 0; j < dimention; ++j) {
boost_matrix (i, j) = matrix (i, j);
}
}
};
double summOffDiagonal2(boost::numeric::ublas::matrix<double> &S) {
double sum = 0;
for (int i = 0; i < S.size1(); i++) {
for (int j = i + 1; j < S.size2(); j++)
{
sum += abs(S(i, j));
}
}
return sum;
}
ublas::matrix<double> remove_duplicates(const ublas::matrix<double>& D)
{
assert(D.size1() == D.size2());
int N = D.size1();
// find duplicates
vector<int> remove;
for(int i=0;i<N;i++) {
bool found=false;
for(int j=0;j<i and not found;j++)
if (D(i,j) == 0.0)
found=true;
if (found)
remove.push_back(i);
}
if (not remove.size()) return D;
cerr<<"removing "<<remove.size()<<" duplicates."<<endl;
// compute mapping of old to new indices
vector<int> indices(N-remove.size());
int k=0;
int p=0;
for(int i=0;i<indices.size();i++)
{
while (p<remove.size() and k==remove[p]) {
k++;
p++;
}
indices[i] = k++;
}
// construct the new matrix
ublas::matrix<double> D2(indices.size(),indices.size());
for(int i=0;i<D2.size1();i++)
for(int j=0;j<D2.size2();j++)
D2(i,j) = D(indices[i],indices[j]);
return D2;
}
void printMatrix(boost::numeric::ublas::matrix<int> matrix) {
for (unsigned int i=0; i < matrix.size1(); i++) {
for (unsigned int j=0; j < matrix.size2(); j++) {
cout << matrix(i, j);
if(j+1 != matrix.size2()) {
cout << "\t";
}
}
cout << endl;
}
}
Matrix <T> convert_boost_matrix_to_munkres_matrix (const boost::numeric::ublas::matrix <T> & boost_matrix)
{
const int dimention = std::min (boost_matrix.size1 (), boost_matrix.size2 () );
Matrix <T> matrix (dimention, dimention);
for (int i = 0; i < dimention; ++i) {
for (int j = 0; j < dimention; ++j) {
matrix (i, j) = boost_matrix (i, j);
}
}
return matrix;
};
void setTrajectoryKeyPosition(double * trajectoryKeyPositions, int * size1, int * size2)
{
if (TRAJECTORYKEYPOSITIONS.size1()*TRAJECTORYKEYPOSITIONS.size2() != 0)
{
std::cout << "You are reinitializing TrajectoryKeyPositions...\n" ;
TRAJECTORYKEYPOSITIONS.resize(0, 0);
}
TRAJECTORYKEYPOSITIONS.resize(*size1, *size2);
for (int i = 0; i < *size1 * (*size2); i++)
TRAJECTORYKEYPOSITIONS(i % (*size1), i / (*size1)) = trajectoryKeyPositions[i];
}
const boost::numeric::ublas::matrix<float>
NBMath::solve(boost::numeric::ublas::matrix<float> A,
const boost::numeric::ublas::matrix<float> &B) {
boost::numeric::ublas::matrix<float> result(A.size2(), B.size1());
try {
boost::numeric::ublas::permutation_matrix<> P(A.size1());
int singularRow = lu_factorize(A, P);
if (singularRow != 0) {
// TODO: This case needs to be dealt with
std::cout << "the system had no solution!" << std::endl;
}
result.assign(B);
lu_substitute(A, P, result);
} catch (std::exception &e) {
std::cout << "This is a shitty error and someone should track it down and fix it"
<< "but right now we need not to segfault - Mexico 2012"
<< e.what() << std::endl;
}
return result;
}
matrix<double> BoostMatrixFacade::invert(const boost::numeric::ublas::matrix<double> matrix)
{
boost::numeric::ublas::matrix<double> inverted(matrix.size1(), matrix.size2());
cpu_invert(matrix, inverted);
for( int i = 0; i < inverted.size1(); i++ )
for( int j = 0; j < inverted.size2(); j++ )
if( isnan(inverted(i, j)) )
inverted(i, j) = 0;
return inverted;
}
void abs(boost::numeric::ublas::matrix<double> &M)
{
int n = M.size1();
for (int k = 0; k < n; k++)
{
for (int i = 0; i < n; i++)
{
M(k, i) = abs(M(k, i));
}
}
}
int cholesky_basic_checked(ublas::matrix<double,F,A> &m, const bool upper)
// Perform Cholesky decomposition, but do not zero out other-triangular part.
// Returns 0 if matrix was actual positive-definite, otherwise it returns a
// LAPACK info value.
{
if (m.size1() != m.size2())
throw LogicalError(ERROR_INFO("Matrix is not square"));
assert(is_symmetric(m));
// Call LAPACK routine
int info;
char uplo = detail::uplo_flag(m, upper);
int size = static_cast<int>(m.size1());
detail::dpotrf_( &uplo, &size, &m.data()[0], &size, &info );
// Check validity of result
if (info < 0)
throw LogicalError(ERROR_INFO("Invalid argument"), info);
return info;
}
boost::python::object
ublas_to_numpy(
const boost::numeric::ublas::matrix< T > & m )
{
//create a numpy array to put it in
boost::python::object result(
array_type(
boost::python::make_tuple( m.size1(), m.size2() ),
dtype ) );
//copy the elements
for( unsigned i = 0; m.size1() != i; ++i )
{
for( unsigned j = 0; m.size2() != j; ++j )
{
result[ boost::python::make_tuple( i, j ) ] = m( i, j );
}
}
return result;
}
const boost::numeric::ublas::matrix<double>
FixedLagSmootherKalmanFilter::CreateTermA(
const int lag,
const int state_size)
{
//
assert(lag > 0 && "Term A is not needed for a lag of zero");
const boost::numeric::ublas::matrix<double> v
= Matrix::SimplifyVectorOfMatrix(CreateComplexTermA(lag,state_size));
assert(lag * state_size == boost::numeric_cast<int>(v.size1()));
assert( 1 * state_size == boost::numeric_cast<int>(v.size2()));
return v;
}
inline void serialize(json_output_handler& os, const boost::numeric::ublas::matrix<double>& A)
{
os.begin_array();
for (size_t i = 0; i < A.size1(); ++i)
{
os.begin_array();
for (size_t j = 0; j < A.size2(); ++j)
{
os.value(A(i, j));
}
os.end_array();
}
os.end_array();
}
void resize(ublas::matrix<int>& M1,int s1,int s2,int clear=0)
{
ublas::matrix<int> M2(s1,s2);
for(int i=0;i<M2.size1();i++)
for(int j=0;j<M2.size2();j++)
M2(i,j) = clear;
for(int i=0;i<M1.size1() and i<M2.size1();i++)
for(int j=0;j< M1.size2() and j<M2.size2();j++)
M2(i,j) = M1(i,j);
M1.swap(M2);
}
int jacobiSync(
boost::numeric::ublas::matrix<double> &S,
boost::numeric::ublas::vector<double> &e,
boost::numeric::ublas::matrix<double> &U,
int &iter,
bool isOptimized)
{
iter = 0;
int col, row;
bool iterating = true;
int n = S.size1();
if (S.size2() != n)
{
return -1;
}
boost::numeric::ublas::matrix<double> M(n, n);
e = boost::numeric::ublas::zero_vector<double>(n);
U = boost::numeric::ublas::identity_matrix<double>(n, n);
while (iterating)
{
M = S;
abs(M);
iter++;
for (int k = 0; k < n; k++)
{
M(k, k) = 0;
}
findMax(M, row, col);
if (row == col)
{
for (int i = 0; i < n; i++) e(i) = S(i, i);
return 0;
}
double Smax = S(row, col);
if (isOptimized) {
rotateColRowJacobi(S, row, col);
}
else {
rotateRowCol(S, U, row, col);
}
//cout<<row<<" row|col "<<col <<" sum: "<< sumOffDiagonal(S) << endl;
//if (Smax < _EPS * norm_frobenius(S)) iterating = false;
if (sumOffDiagonal(S) < _EPS) iterating = false;
}
for (int i = 0; i < n; i++) e(i) = S(i, i);
return 0;
}
ScalarType matrix_compare(ublas::matrix<ScalarType>& res,
ublas::matrix<ScalarType>& ref)
{
ScalarType diff = 0.0;
ScalarType mx = 0.0;
for(std::size_t i = 0; i < res.size1(); i++)
{
for(std::size_t j = 0; j < res.size2(); j++)
{
diff = std::max(diff, std::abs(res(i, j) - ref(i, j)));
mx = std::max(mx, res(i, j));
}
}
return diff / mx;
}
