bool checkConsistency(Matrix &a)
{
gsl_matrix *tmp;
yarp::gsl::GslMatrix tmpGSL(a);
tmp=(gsl_matrix *)(tmpGSL.getGslMatrix());
bool ret=true;
if (tmp->size1!=a.rows())
ret=false;
if (tmp->size2!=a.cols())
ret=false;
if (tmp->block->size!=a.cols()*a.rows())
ret=false;
if (tmp->data!=a.data())
ret=false;
if (tmp->block->data!=a.data())
ret=false;
return ret;
}
void M_Add4(Matrix<Scalar>& M1, Matrix<Scalar>& M2, Matrix<Scalar>& C, double x, bool sequential, Scalar beta) {
const int strideM1 = M1.stride();
const int strideM2 = M2.stride();
const int strideC = C.stride();
const Scalar *dataM1 = M1.data();
const Scalar *dataM2 = M2.data();
Scalar *dataC = C.data();
if (beta != Scalar(0.0)) {
#ifdef _PARALLEL_
# pragma omp parallel for if(!sequential)
#endif
for (int j = 0; j < C.n(); ++j) {
for (int i = 0; i < C.m(); ++i) {
dataC[i + j * strideC] = dataM1[i + j * strideM1] + dataM2[i + j * strideM2] + beta * dataC[i + j * strideC];
}
}
} else {
#ifdef _PARALLEL_
# pragma omp parallel for if(!sequential)
#endif
for (int j = 0; j < C.n(); ++j) {
for (int i = 0; i < C.m(); ++i) {
dataC[i + j * strideC] = dataM1[i + j * strideM1] + dataM2[i + j * strideM2];
}
}
}
}
inline
bool operator==(const Matrix& A, const Matrix& B)
{
bool rv = (&A == &B) ||
std::equal(A.data().begin(), A.data().end(), B.data().begin());
return rv;
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[]) {
if (nrhs != 5) {
mexErrMsgIdAndTxt("Drake:cartesian2cylindricalmex:IncorrectInputs",
"Usage [x_cartesian, v_cartesian, J, Jdotv] = "
"cartesian2cylindrical(cylinder_axis, cylinder_x_dir,"
"cylinder_origin, x_cylinder, v_cylinder)");
}
if (!mxIsNumeric(prhs[0]) || mxGetNumberOfElements(prhs[0]) != 3) {
mexErrMsgIdAndTxt("Drake:cartesian2cylindricalmex:InvalidInput",
"cylinder_axis should be a 3 x 1 vector");
}
Vector3d cylinder_axis;
memcpy(cylinder_axis.data(), mxGetPr(prhs[0]), sizeof(double) * 3);
if (!mxIsNumeric(prhs[1]) || mxGetNumberOfElements(prhs[1]) != 3) {
mexErrMsgIdAndTxt("Drake:cartesian2cylindricalmex:InvalidInput",
"cylinder_x_dir should be a 3 x 1 vector");
}
Vector3d cylinder_x_dir;
memcpy(cylinder_x_dir.data(), mxGetPr(prhs[1]), sizeof(double) * 3);
if (!mxIsNumeric(prhs[2]) || mxGetNumberOfElements(prhs[2]) != 3) {
mexErrMsgIdAndTxt("Drake:cartesian2cylindricalmex:InvalidInput",
"cylinder_origin should be a 3 x 1 vector");
}
Vector3d cylinder_origin;
memcpy(cylinder_origin.data(), mxGetPr(prhs[2]), sizeof(double) * 3);
if (!mxIsNumeric(prhs[3]) || mxGetM(prhs[3]) != 6 || mxGetN(prhs[3]) != 1) {
mexErrMsgIdAndTxt("Drake:cartesian2cylindricalmex:InvalidInput",
"x should be a 6 x 1 vector");
}
Matrix<double, 6, 1> x_cartesian;
memcpy(x_cartesian.data(), mxGetPr(prhs[3]), sizeof(double) * 6);
if (!mxIsNumeric(prhs[4]) || mxGetM(prhs[4]) != 6 || mxGetN(prhs[3]) != 1) {
mexErrMsgIdAndTxt("Drake:cartesian2cylindricalmex:InvalidInput",
"v should be a 6 x 1 vector");
}
Matrix<double, 6, 1> v_cartesian;
memcpy(v_cartesian.data(), mxGetPr(prhs[4]), sizeof(double) * 6);
cylinder_axis = cylinder_axis / cylinder_axis.norm();
cylinder_x_dir = cylinder_x_dir / cylinder_x_dir.norm();
if (abs(cylinder_axis.transpose() * cylinder_x_dir) > 1e-10) {
mexErrMsgIdAndTxt("Drake:cartesian2cylindricalmex:InvalidInput",
"cylinder_x_dir and cylinder_axis should be "
"perpendicular to each othter");
}
Matrix<double, 6, 1> x_cylinder;
Matrix<double, 6, 1> v_cylinder;
Matrix<double, 6, 6> J;
Matrix<double, 6, 1> Jdotv;
drake::math::cartesian2cylindrical(cylinder_axis, cylinder_x_dir,
cylinder_origin, x_cartesian, v_cartesian,
x_cylinder, v_cylinder, J, Jdotv);
plhs[0] = mxCreateDoubleMatrix(6, 1, mxREAL);
memcpy(mxGetPr(plhs[0]), x_cylinder.data(), sizeof(double) * 6);
plhs[1] = mxCreateDoubleMatrix(6, 1, mxREAL);
memcpy(mxGetPr(plhs[1]), v_cylinder.data(), sizeof(double) * 6);
plhs[2] = mxCreateDoubleMatrix(6, 6, mxREAL);
memcpy(mxGetPr(plhs[2]), J.data(), sizeof(double) * 36);
plhs[3] = mxCreateDoubleMatrix(6, 1, mxREAL);
memcpy(mxGetPr(plhs[3]), Jdotv.data(), sizeof(double) * 6);
}
Matrix Quaternion::matrix()
{
Matrix ret;
float fTx = 2.0f * x;
float fTy = 2.0f * y;
float fTz = 2.0f * z;
float fTwx = fTx * w;
float fTwy = fTy * w;
float fTwz = fTz * w;
float fTxx = fTx * x;
float fTxy = fTy * x;
float fTxz = fTz * x;
float fTyy = fTy * y;
float fTyz = fTz * y;
float fTzz = fTz * z;
ret.data()[0] = 1.0f - ( fTyy + fTzz );
ret.data()[1] = fTxy - fTwz;
ret.data()[2] = fTxz + fTwy;
ret.data()[4] = fTxy + fTwz;
ret.data()[5] = 1.0f - ( fTxx + fTzz );
ret.data()[6] = fTyz - fTwx;
ret.data()[8] = fTxz - fTwy;
ret.data()[9] = fTyz + fTwx;
ret.data()[10] = 1.0f - ( fTxx + fTyy );
ret.data()[15] = 1.0f;
return ret;
}
void Detector::ExtractPointsInROIs(/*Vector<Vector<Vector<double> > > &all_points_in_ROIs*/Vector<ROI> &all_ROIs, const Matrix<int> &mat_2D_pos_x, const Matrix<int> &mat_2D_pos_y, const PointCloud& point_cloud, const Matrix<int> &labeled_ROIs)
{
// int number_of_ROIs = all_points_in_ROIs.getSize();
int number_of_ROIs = all_ROIs.getSize();
// for(int i = 0; i < number_of_ROIs; i++)
// {
// all_points_in_ROIs(i).reserveCapacity(50000);
// }
Vector<ROIBound> min_maxs(number_of_ROIs);
ROIBound* roi_bound;
int roi_ind;
int mat_size = mat_2D_pos_x.x_size()*mat_2D_pos_x.y_size();
for(int i = 0; i < mat_size; i++)
{
if(mat_2D_pos_x.data()[i] >= 0)
{
roi_ind = labeled_ROIs(mat_2D_pos_x.data()[i], mat_2D_pos_y.data()[i])-1;
if(roi_ind > -1)
{
// all_points_in_ROIs(pos_in_proj).pushBack(Vector<double>(point_cloud.X(i), point_cloud.Y(i), point_cloud.Z(i)));
all_ROIs(roi_ind).has_any_point = true;
double x = point_cloud.X(i), y = point_cloud.Y(i), z = point_cloud.Z(i);
roi_bound = &min_maxs(roi_ind);
if(roi_bound->min_x > x)
{
roi_bound->min_x = x;
all_ROIs(roi_ind).ind_min_x = i;
}
if(roi_bound->max_x < x)
{
roi_bound->max_x = x;
all_ROIs(roi_ind).ind_max_x = i;
}
if(roi_bound->min_y > y)
{
roi_bound->min_y = y;
all_ROIs(roi_ind).ind_min_y = i;
}
if(roi_bound->max_y < y)
{
roi_bound->max_y = y;
all_ROIs(roi_ind).ind_max_y = i;
}
if(roi_bound->min_z > z)
{
roi_bound->min_z = z;
all_ROIs(roi_ind).ind_min_z = i;
}
if(roi_bound->max_z < z)
{
roi_bound->max_z = z;
all_ROIs(roi_ind).ind_max_z = i;
}
}
}
}
}
inline double element_sum(const Matrix& M1)
{
const int size = M1.data().size();
const double * __restrict__ m1 = M1.data().begin();
double sum = 0;
for(int i=0;i<size;i++)
sum += m1[i];
return sum;
}
Matrix yarp::math::pile(const Matrix &m1, const Matrix &m2)
{
int c = m1.cols();
yAssert(c==m2.cols());
int r1 = m1.rows();
int r2 = m2.rows();
Matrix res(r1+r2, c);
cblas_dcopy(r1*c, m1.data(), 1, res.data(), 1); // copy first r1 rows
cblas_dcopy(r2*c, m2.data(), 1, res[r1], 1); // copy last r2 rows
return res;
}
Matrix yarp::math::operator*(const Matrix &a, const Matrix &b)
{
yAssert(a.cols()==b.rows());
Matrix c(a.rows(), b.cols());
cblas_dgemm (CblasRowMajor, CblasNoTrans, CblasNoTrans,
c.rows(), c.cols(), a.cols(),
1.0, a.data(), a.cols(), b.data(), b.cols(), 0.0,
c.data(), c.cols());
return c;
}
inline void element_prod_assign(Matrix& M1,const Matrix& M2)
{
assert(M1.size1() == M2.size1());
assert(M1.size2() == M2.size2());
const int size = M1.data().size();
double * __restrict__ m1 = M1.data().begin();
const double * __restrict__ m2 = M2.data().begin();
for(int i=0;i<size;i++)
m1[i] *= m2[i];
}
template <typename num_t> void Tensor<num_t>::setBlock(Matrix<num_t>& mat) {
if (all_elem_.data() != mat.data()) {
// std::cout << "ENTER setBlock" << std::endl;
if (mat.row() == getInSize() && mat.col() == getOutSize()) {
std::copy(mat.data(),mat.data()+mat.row()*mat.col(),all_elem_.data());
} else {
std::cout << "The Matrix do not correspond with the tensor bonds size\nPass the operation" << std::endl;
}
}
return;
}
void reset(int fft_dim, int howmany)
{
myfft.set_defaults(fft_dim,howmany);
in.resize(myfft(FFT_NUMBER_OF_TRANSFORMS),myfft(FFT_IN_DISTANCE));
out.resize(myfft(FFT_NUMBER_OF_TRANSFORMS),myfft(FFT_OUT_DISTANCE));
std::vector<space_type> sample(fft_dim);
real_type phase=2*M_PI/static_cast<real_type>(fft_dim);
for(int i=0; i<sample.size(); ++i)
sample[i]=0.5*std::sin(phase*i)+0.1*std::sin(phase*2*i)+0.3*std::sin(phase*3*i)+0.5*std::sin(phase*4.1*i);;
for(int i=0; i<howmany; ++i)
std::copy(sample.begin(),sample.end(),in[i]);
myfft.create(in.data(),out.data());
}
void BGR2RGB(Matrix& maRed, Matrix& maGreen, Matrix& maBlue)
{
for(int i = 0; i < ivHeight; ++i){
for(int j = 0; j < ivWidth; ++j){
curImage.at<cv::Vec3b>(i,j).val[2] = maRed.data()[i*ivWidth+j];
curImage.at<cv::Vec3b>(i,j).val[1] = maGreen.data()[i*ivWidth+j];
curImage.at<cv::Vec3b>(i,j).val[0] = maBlue.data()[i*ivWidth+j];
}
}
//at this place you could save the images using
//cvSaveImage(filename, curImage);
}
void Canny::roundTheta(const Matrix<float>& theta, Matrix<unsigned char>& dst) const {
#pragma omp parallel for
for(int y = 2; y < ((int)dst.height() - 2); ++y)
for(int x = 2; x < ((int)dst.width() - 2); ++x) {
const float t = (theta.data()[x + y*theta.width()]/M_PI)*180.0f + 180.0f;
unsigned char& d = dst.data()[x + y*dst.width()];
if((t > 0.0f && t <= 22.5f) || (t > 157.5f && t <= 202.5f) || (t > 337.5f && t <= 360.0f)) d = 0;
if((t > 22.5f && t <= 67.5f) || (t > 202.5f && t <= 247.5f)) d = 1;
if((t > 67.5f && t <= 112.5f) || (t > 247.5f && t <= 292.5f)) d = 2;
if((t > 112.5f && t <= 157.5f) || (t > 292.5f && t <= 337.5f)) d = 3;
}
}
void S_Add1(Matrix<Scalar>& S1, Matrix<Scalar>& C, double x, bool sequential) {
const int strideS1 = S1.stride();
const int strideC = C.stride();
const Scalar *dataS1 = S1.data();
Scalar *dataC = C.data();
#ifdef _PARALLEL_
# pragma omp parallel for if(!sequential)
#endif
for (int j = 0; j < C.n(); ++j) {
for (int i = 0; i < C.m(); ++i) {
dataC[i + j * strideC] = dataS1[i + j * strideS1];
}
}
}
void checkEmpty() {
report(0,"check data() when matrix is empty...");
Matrix m;
m.resize(0,0);
checkTrue(m.data()==nullptr, "size 0x0 => null data()");
m.resize(0,5);
checkTrue(m.data()==nullptr, "size 0x5 => null data()");
m.resize(5,0);
checkTrue(m.data()==nullptr, "size 5x0 => null data()");
m.resize(5,5);
checkTrue(m.data()!=nullptr, "size 5x5 => non-null data()");
// This is *not* redundant with earlier test
m.resize(0,0);
checkTrue(m.data()==nullptr, "size 0x0 => null data()");
}
void Canny::sobel(const Matrix<float>& src, Matrix<float>& G, Matrix<float>& theta) const {
#define I(x, y) src.data()[(x) + (y)*src.width()]
#pragma omp parallel for
for(int y = 2; y < ((int)src.height() - 2); ++y)
for(int x = 2; x < ((int)src.width() - 2); ++x) {
const float gx = I(x + 1, y - 1) - I(x - 1, y - 1) + 2.0f*I(x + 1, y) - 2.0f*I(x - 1, y) + I(x + 1, y + 1) - I(x - 1, y + 1);
const float gy = I(x - 1, y + 1) - I(x - 1, y - 1) + 2.0f*I(x, y + 1) - 2.0f*I(x, y - 1) + I(x + 1, y + 1) - I(x + 1, y - 1);
G.data()[x + y*G.width()] = sqrtf(gx*gx + gy*gy);
theta.data()[x + y*theta.width()] = atan2f(gy,gx);
}
#undef I
}
void mexFunction(int nlhs, mxArray* plhs[], int nrhs, const mxArray* prhs[])
{
if(nrhs != 5)
{
mexErrMsgIdAndTxt("Drake:testQuatmex:BadInputs","Usage [r,dr,e,ed,quat,dquat,q3,dq3,w,dw] = testQuatmex(q1,q2,axis,u,v)");
}
Vector4d q1;
Vector4d q2;
memcpy(q1.data(),mxGetPr(prhs[0]),sizeof(double)*4);
memcpy(q2.data(),mxGetPr(prhs[1]),sizeof(double)*4);
Vector4d r = quatDiff(q1,q2);
Matrix<double,4,8> dr = dquatDiff(q1,q2);
plhs[0] = mxCreateDoubleMatrix(4,1,mxREAL);
plhs[1] = mxCreateDoubleMatrix(4,8,mxREAL);
memcpy(mxGetPr(plhs[0]),r.data(),sizeof(double)*4);
memcpy(mxGetPr(plhs[1]),dr.data(),sizeof(double)*4*8);
Vector3d axis;
memcpy(axis.data(),mxGetPr(prhs[2]),sizeof(double)*3);
double e = quatDiffAxisInvar(q1,q2,axis);
Matrix<double,1,11> de = dquatDiffAxisInvar(q1,q2,axis);
plhs[2] = mxCreateDoubleScalar(e);
plhs[3] = mxCreateDoubleMatrix(1,11,mxREAL);
memcpy(mxGetPr(plhs[3]),de.data(),sizeof(double)*11);
Vector3d u,v;
Vector4d quat;
Matrix<double,4,6> dquat;
memcpy(u.data(),mxGetPr(prhs[3]),sizeof(double)*3);
memcpy(v.data(),mxGetPr(prhs[4]),sizeof(double)*3);
Vector4d q3 = quatProduct(q1,q2);
Matrix<double,4,8> dq3 = dquatProduct(q1,q2);
plhs[4] = mxCreateDoubleMatrix(4,1,mxREAL);
plhs[5] = mxCreateDoubleMatrix(4,8,mxREAL);
memcpy(mxGetPr(plhs[4]),q3.data(),sizeof(double)*4);
memcpy(mxGetPr(plhs[5]),dq3.data(),sizeof(double)*4*8);
Vector3d w = quatRotateVec(q1,u);
Matrix<double,3,7> dw = dquatRotateVec(q1,u);
plhs[6] = mxCreateDoubleMatrix(3,1,mxREAL);
plhs[7] = mxCreateDoubleMatrix(3,7,mxREAL);
memcpy(mxGetPr(plhs[6]),w.data(),sizeof(double)*3);
memcpy(mxGetPr(plhs[7]),dw.data(),sizeof(double)*3*7);
}
IGL_INLINE void igl::matlab::parse_rhs_double(
const mxArray *prhs[],
Eigen::PlainObjectBase<DerivedV> & V)
{
using namespace std;
using namespace Eigen;
// set number of mesh vertices
const int n = mxGetM(prhs[0]);
// set vertex position pointers
double * Vp = mxGetPr(prhs[0]);
const int dim = mxGetN(prhs[0]);
typedef typename DerivedV::Scalar Scalar;
Matrix<Scalar, DerivedV::ColsAtCompileTime, DerivedV::RowsAtCompileTime, RowMajor> VT;
Scalar * V_data;
if(DerivedV::IsRowMajor)
{
VT.resize(dim,n);
V_data = VT.data();
}else
{
V.resize(n,dim);
V_data = V.data();
}
copy(Vp,Vp+n*dim,V_data);
if(DerivedV::IsRowMajor)
{
V = VT.transpose();
}
}
void mmf::OptSO3ApproxGD::computeSuffcientStatistics()
{
// compute rotations to north pole
Matrix<float,2*6,3,RowMajor> Rnorths(2*6,3);
for (uint32_t j=0; j<6; ++j)
{
Rnorths.middleRows<2>(j*2) = S2_.north_R_TpS2(qKarch_.col(j)).topRows<2>();
// cout<<qKarch_.col(j).transpose()<<endl;
// cout<<Rnorths.middleRows<2>(j*2)<<endl;
// cout<<"----"<<endl;
}
// cout<<Rnorths<<endl;
Matrix<float,7,6,ColMajor> SSs;
sufficientStatisticsOnTpS2GPU(qKarch_.data(), d_mu_karch_,
Rnorths.data(), d_Rnorths_, this->cld_.d_x(), this->cld_.d_z() ,
this->cld_.N(), SSs.data(), d_SSs_);
//cout<<SSs<<endl;
for (uint32_t j=0; j<6; ++j)
{
xSums_.col(j) = SSs.block<2,1>(0,j);
Ss_[j](0,0) = SSs(2,j);
Ss_[j](0,1) = SSs(3,j);
Ss_[j](1,0) = SSs(4,j);
Ss_[j](1,1) = SSs(5,j);
Ns_(j) = SSs(6,j);
//cout<<"@j="<<j<<"\t"<< Ss_[j]<<endl;
}
// cout<<xSums_<<endl;
// cout<<Ns_<<endl;
}
void FromRGB(Matrix& maRed, Matrix& maGreen, Matrix& maBlue)
{
for(int i = 0; i < ivWidth*ivHeight; ++i){
curImage->imageData[3*i+2] = maRed.data()[i];
curImage->imageData[3*i+1] = maGreen.data()[i];
curImage->imageData[3*i+0] = maBlue.data()[i];
}
//at this place you could save the images using
//cvSaveImage(filename, curImage);
if(mouseMode == MOUSE_MODE_MARKER)
{
CvPoint pt1; pt1.x = mouseBox.x; pt1.y = mouseBox.y;
CvPoint pt2; pt2.x = mouseBox.x + mouseBox.width; pt2.y = mouseBox.y + mouseBox.height;
cvRectangle(curImage, pt1, pt2, CV_RGB(0,0,255));
}
}
SpdMatrix LTL(const Matrix &L){
Matrix ans(L);
cblas_dtrmm(CblasColMajor, CblasLeft, CblasLower, CblasTrans,
CblasNonUnit, L.nrow(), L.ncol(), 1.0,
L.data(), L.nrow(), ans.data(), ans.nrow());
return ans;
}
void Canny::detectMax(const Matrix<float>& G, const Matrix<unsigned char>& dir, float tresh, Image& dst) const {
#define I(x, y) (G.data()[(x) + (y)*G.width()])
#pragma omp parallel for
for(int y = 2; y < ((int)dst.height() - 2); ++y)
for(int x = 2; x < ((int)dst.width() - 2); ++x) {
switch(dir.data()[x + y*dir.width()]) {
case 0:
if(I(x - 1, y) < I(x, y) && I(x + 1, y) < I(x, y) && I(x, y) > tresh) dst.data()[x + y*dst.width()] = 255;
else dst.data()[x + y*dst.width()] = 0;
break;
case 1:
if(I(x - 1, y - 1) < I(x, y) && I(x + 1, y + 1) < I(x, y) && I(x, y) > tresh) dst.data()[x + y*dst.width()] = 255;
else dst.data()[x + y*dst.width()] = 0;
break;
case 2:
if(I(x, y - 1) < I(x, y) && I(x, y + 1) < I(x, y) && I(x, y) > tresh) dst.data()[x + y*dst.width()] = 255;
else dst.data()[x + y*dst.width()] = 0;
break;
case 3:
if(I(x - 1, y + 1) < I(x, y) && I(x + 1, y - 1) < I(x, y) && I(x, y) > tresh) dst.data()[x + y*dst.width()] = 255;
else dst.data()[x + y*dst.width()] = 0;
break;
}
}
#undef I
}
void HarrisExtractorBucket::localMaximaList_(const Matrix<float>& src, unsigned int camera, vector<HarrisPoint_>& dst) const {
const int xMin = 3, xMax = src.width() - 3;
const int yMin = 3, yMax = src.height() - 3;
const int w = src.width();
for(int y = yMin + 1; y < yMax - 1; y++) {
for(int x = xMin + 1; x < xMax - 1; x++) {
const float* P = src.data() + x + y*src.width(), V = P[0];
if(Configuration::Instance().minValue() < V) {
if(P[1] < V) {
if((P[-1] < V) && (P[w] < V) && (P[-w] < V) && (P[- 1 + w] < V) && (P[1 + w] < V)
&& (P[-1 - w] < V) && (P[1 - w] < V)) {
HarrisPoint_ p;
p.camera = camera;
p.point = Point2D(x, y);
dst.push_back(p);
}
x++; //si P[0] > P[1] alors P[1] ne peut pas etre un max local, inutile de le tester
}
}
}
}
}
void Window::resize(uint32_t width, uint32_t height)
{
SDL_GL_SetAttribute(SDL_GL_DOUBLEBUFFER, 1);
_windowSurface = SDL_SetVideoMode(width, height, 0, SDL_OPENGL);
if (!_windowSurface)
{
// that didn't work...
// throw?
}
_width = width;
_height = height;
// reset the OpenGL context.
glEnable(GL_TEXTURE_2D);
glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
glViewport(0, 0, width, height);
glClear(GL_COLOR_BUFFER_BIT);
Matrix projection;
Matrix::createOrthographic(0.0f, width, height, 0.0f, -1.0f, 1.0, projection);
glMatrixMode(GL_PROJECTION);
glLoadMatrixf(projection.data());
glMatrixMode(GL_MODELVIEW);
glLoadMatrixf(Matrix::identity().data());
}
bool yarp::sig::submatrix(const Matrix &in, Matrix &out, int r1, int r2, int c1, int c2)
{
double *t=out.data();
const double *i=in.data()+in.cols()*r1+c1;
const int offset=in.cols()-(c2-c1+1);
for(int r=0;r<=(r2-r1);r++)
{
for(int c=0;c<=(c2-c1);c++)
{
*t++=*i++;
}
i+=offset;
}
return true;
}
void Detector_Seg::PreprocessROIs(Matrix<int> &labeled_ROIs, Vector<ROI_SEG> &all_ROIs)
{
KConnectedComponentLabeler ccl(Globals::region_size_threshold, labeled_ROIs.data(), labeled_ROIs.x_size(), labeled_ROIs.y_size());
ccl.Process(); // Main function
ROI_SEG::ExtractROIs(all_ROIs, labeled_ROIs, ccl.m_ObjectNumber);
}
void knnSearch(const Matrix<double, N, 1, Options, MaxRows, MaxCols>& query,
size_t k, std::vector<int>& indices,
bool remove1NN = false)
{
if (data_.cols != query.size())
throw std::runtime_error("Dimension of query vector do not match \
dimension of input feature space!");
std::vector<double> sqDists;
if (remove1NN)
{
knnSearch(query.data(), k+1, indices, sqDists);
indices.erase(indices.begin());
sqDists.erase(sqDists.begin());
}
else
knnSearch(query.data(), k, indices, sqDists);
}
/// Upload an Eigen matrix as a vertex buffer object (refreshing it as needed)
template <typename Matrix> void uploadAttrib(const std::string &name, const Matrix &M, int version = -1) {
uint32_t compSize = sizeof(typename Matrix::Scalar);
GLuint glType = (GLuint) type_traits<typename Matrix::Scalar>::type;
bool integral = (bool) type_traits<typename Matrix::Scalar>::integral;
uploadAttrib(name, M.size(), M.rows(), compSize,
glType, integral, (const uint8_t *) M.data(), version);
}
inline vector<T> to_array_1d(const Matrix<T, R, C> & matrix) {
const T* datap = matrix.data();
int size = matrix.size();
vector<T> result(size);
for (int i=0; i < size; i++)
result[i] = datap[i];
return result;
}
