void FiffSimulator::doContinousHPI(MatrixXf& matData)
{
//This only works with babyMEG HPI channels 400 ... 407
if(m_pFiffInfo && m_pHPIWidget && matData.rows() >= 407) {
if(m_pHPIWidget->wasLastFitOk()) {
// Load device to head transformation matrix from Fiff info
QMatrix3x3 rot;
for(int ir = 0; ir < 3; ir++) {
for(int ic = 0; ic < 3; ic++) {
rot(ir,ic) = m_pFiffInfo->dev_head_t.trans(ir,ic);
}
}
QQuaternion quatHPI = QQuaternion::fromRotationMatrix(rot);
// Write rotation quaternion to HPI Ch #1~3
matData.row(401) = MatrixXf::Constant(1,matData.cols(), quatHPI.x());
matData.row(402) = MatrixXf::Constant(1,matData.cols(), quatHPI.y());
matData.row(403) = MatrixXf::Constant(1,matData.cols(), quatHPI.z());
// Write translation vector to HPI Ch #4~6
matData.row(404) = MatrixXf::Constant(1,matData.cols(), m_pFiffInfo->dev_head_t.trans(0,3));
matData.row(405) = MatrixXf::Constant(1,matData.cols(), m_pFiffInfo->dev_head_t.trans(1,3));
matData.row(406) = MatrixXf::Constant(1,matData.cols(), m_pFiffInfo->dev_head_t.trans(2,3));
// Write GOF to HPI Ch #7
// Write goodness of fit (GOF)to HPI Ch #7
float dpfitError = 0.0;
float GOF = 1 - dpfitError;
matData.row(407) = MatrixXf::Constant(1,matData.cols(), GOF);
}
}
}
void NeighbourJoining::calcNewD(MatrixXf& currentD, MatrixXi& rowsID, const Pair& p) {
//calculates distances to new node
int j = 0;
for (int i = 0; i < numCurrentNodes - 1; ++i) {
if (i == p.i)
j++;
currentD(numCurrentNodes, i) = (currentD(p.i, i + j) + currentD(p.j, i + j) - currentD(p.i, p.j)) / 2;
currentD(i, numCurrentNodes) = currentD(numCurrentNodes, i);
}
//cout << "distances to new node: " << currentD.row(numCurrentNodes).head(numCurrentNodes-1) <<endl;
//swaps rows and columns so that the closest pair nodes go right and at the bottom of the matrix
currentD.row(p.i).head(numCurrentNodes - 1).swap(
currentD.row(numCurrentNodes - 1).head(numCurrentNodes - 1));
currentD.col(p.i).head(numCurrentNodes - 1).swap(
currentD.col(numCurrentNodes - 1).head(numCurrentNodes - 1));
currentD.row(p.j).head(numCurrentNodes - 1).swap(
currentD.row(numCurrentNodes).head(numCurrentNodes - 1));
currentD.col(p.j).head(numCurrentNodes - 1).swap(
currentD.col(numCurrentNodes).head(numCurrentNodes - 1));
currentD.diagonal().setZero();
//cout << "new Matrix:" << endl; printMatrix(currentD);
//adjusts node IDs to new matrix indices
int newNode = 2 * numObservableNodes - numCurrentNodes;
rowsID.row(p.i).swap(rowsID.row(numCurrentNodes - 1));
rowsID.row(p.j).swap(rowsID.row(newNode));
//cout << "rowsID: " << rowsID.transpose(); cout << endl;
}
void call_ref()
{
VectorXcf ca = VectorXcf::Random(10);
VectorXf a = VectorXf::Random(10);
RowVectorXf b = RowVectorXf::Random(10);
MatrixXf A = MatrixXf::Random(10,10);
RowVector3f c = RowVector3f::Random();
const VectorXf& ac(a);
VectorBlock<VectorXf> ab(a,0,3);
const VectorBlock<VectorXf> abc(a,0,3);
VERIFY_EVALUATION_COUNT( call_ref_1(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(b,b.transpose()), 0);
// call_ref_1(ac,a<c); // does not compile because ac is const
VERIFY_EVALUATION_COUNT( call_ref_1(ab,ab), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(a.head(4),a.head(4)), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(abc,abc), 0);
VERIFY_EVALUATION_COUNT( call_ref_1(A.col(3),A.col(3)), 0);
// call_ref_1(A.row(3),A.row(3)); // does not compile because innerstride!=1
VERIFY_EVALUATION_COUNT( call_ref_3(A.row(3),A.row(3).transpose()), 0);
VERIFY_EVALUATION_COUNT( call_ref_4(A.row(3),A.row(3).transpose()), 0);
// call_ref_1(a+a, a+a); // does not compile for obvious reason
MatrixXf tmp = A*A.col(1);
VERIFY_EVALUATION_COUNT( call_ref_2(A*A.col(1), tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_2(ac.head(5),ac.head(5)), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(ac,ac), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(ab,ab), 0);
VERIFY_EVALUATION_COUNT( call_ref_2(a.head(4),a.head(4)), 0);
tmp = a+a;
VERIFY_EVALUATION_COUNT( call_ref_2(a+a,tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_2(ca.imag(),ca.imag()), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_4(ac.head(5),ac.head(5)), 0);
tmp = a+a;
VERIFY_EVALUATION_COUNT( call_ref_4(a+a,tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_4(ca.imag(),ca.imag()), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(a.head(3),a.head(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(A,A), 0);
// call_ref_5(A.transpose(),A.transpose()); // does not compile because storage order does not match
VERIFY_EVALUATION_COUNT( call_ref_5(A.block(1,1,2,2),A.block(1,1,2,2)), 0);
VERIFY_EVALUATION_COUNT( call_ref_5(b,b), 0); // storage order do not match, but this is a degenerate case that should work
VERIFY_EVALUATION_COUNT( call_ref_5(a.row(3),a.row(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(a,a), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(a.head(3),a.head(3)), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(A.row(3),A.row(3)), 1); // evaluated into a temp thouth it could be avoided by viewing it as a 1xn matrix
tmp = A+A;
VERIFY_EVALUATION_COUNT( call_ref_6(A+A,tmp), 1); // evaluated into a temp
VERIFY_EVALUATION_COUNT( call_ref_6(A,A), 0);
VERIFY_EVALUATION_COUNT( call_ref_6(A.transpose(),A.transpose()), 1); // evaluated into a temp because the storage orders do not match
VERIFY_EVALUATION_COUNT( call_ref_6(A.block(1,1,2,2),A.block(1,1,2,2)), 0);
VERIFY_EVALUATION_COUNT( call_ref_7(c,c), 0);
}
bool singleModelRANSAC(const MatrixXf &data, int M, MatrixXf &inlier) {
int maxdegen = 10;
int dataSize = data.rows();
int psize = 4;
MatrixXf x1 = data.block(0, 0, data.rows(), 3);
MatrixXf x2 = data.block(0, 3, data.rows(), 3);
vector<int> sample;
MatrixXf pts1(4, 3);
MatrixXf pts2(4, 3);
int maxInlier = -1;
MatrixXf bestResidue;
for (int m = 0; m < M; m++) {
int degencount = 0;
int isdegen = 1;
while (isdegen==1 && degencount < maxdegen) {
degencount ++;
RandomSampling(psize, dataSize, sample);
for (int i = 0; i < psize; i++) {
pts1.row(i) = x1.row(sample[i]);
pts2.row(i) = x2.row(sample[i]);
}
if (sampleValidTest(pts1, pts2))
isdegen = 0;
}
if (isdegen) {
cout << "Cannot find valid p-subset" << endl;
return false;
}
Matrix3f local_H;
MatrixXf local_A;
fitHomography(pts1, pts2, local_H, local_A);
MatrixXf residue;
computeHomographyResidue(x1, x2, local_H, residue);
int inlierCount = (residue.array() < THRESHOLD).count();
if (inlierCount > maxInlier) {
maxInlier = inlierCount;
bestResidue = residue;
}
}
inlier.resize(maxInlier, data.cols());
int transferCounter = 0;
for (int i = 0; i < dataSize; i++) {
if (bestResidue(i) < THRESHOLD) {
inlier.row(transferCounter) = data.row(i);
transferCounter++;
}
}
if (transferCounter != maxInlier) {
cout << "RANSAC result size does not match!!!!" << endl;
return false;
}
return true;
}
void fitHomography(MatrixXf pts1, MatrixXf pts2, Matrix3f &H, MatrixXf &A) {
int psize = pts1.rows();
A.resize(psize*2, 9);
for (auto i = 0; i < psize; i++) {
Vector3f p1 = pts1.row(i);
Vector3f p2 = pts2.row(i);
A.row(i*2) << 0, 0, 0, -p1[0], -p1[1], -p1[2], p2[1]*p1[0], p2[1]*p1[1], p2[1]*p1[2];
A.row(i*2+1) << p1[0], p1[1], p1[2], 0, 0, 0, -p2[0]*p1[0], -p2[0]*p1[1], -p2[0]*p1[2];
}
JacobiSVD<MatrixXf, HouseholderQRPreconditioner> svd(A, ComputeFullV);
MatrixXf V = svd.matrixV();
VectorXf h = V.col(V.cols()-1);
H = rollVector9f(h);
}
MatrixXf interp2f(const VectorXf& xNew, const VectorXf& xOld, const MatrixXf& yOld) {
int nNew = xNew.size();
int nOld = xOld.size();
MatrixXf yNew(nNew, yOld.cols());
VectorXi new2old = searchsorted(xNew, xOld);
for (int iNew=0; iNew < nNew; iNew++) {
int iOldAbove = new2old(iNew);
if (iOldAbove == 0)
yNew.row(iNew) = yOld.row(0);
else if (iOldAbove == nOld)
yNew.row(iNew) = yOld.row(nOld-1);
else {
float t = (xNew(iNew) - xOld(iOldAbove-1)) / (xOld(iOldAbove) - xOld(iOldAbove-1));
yNew.row(iNew) = yOld.row(iOldAbove-1)*(1-t) + yOld.row(iOldAbove)*t;
}
}
return yNew;
}
BoundingBox MeshRenderer::getBoundingBox() {
std::vector<Vector3f> coordinates;
for(uint i = 0; i < getNrOfInputData(); i++) {
BoundingBox transformedBoundingBox = mMeshToRender[i]->getTransformedBoundingBox();
MatrixXf corners = transformedBoundingBox.getCorners();
for(uint j = 0; j < 8; j++) {
coordinates.push_back((Vector3f)corners.row(j));
}
}
return BoundingBox(coordinates);
}
void filterPointAtInfinity(MatrixXf &pts1, MatrixXf &pts2) {
int finiteCount = 0;
for (int i = 0; i < pts1.rows(); i++) {
if (abs(pts1(i, 2)) > FLT_EPSILON && abs(pts2(i, 2) > FLT_EPSILON))
finiteCount++;
}
MatrixXf temp_pts1, temp_pts2;
temp_pts1.resize(finiteCount, pts1.cols());
temp_pts2.resize(finiteCount, pts2.cols());
int idx = 0;
for (int i = 0; i < pts1.rows(); i++) {
if (abs(pts1(i, 2)) > FLT_EPSILON && abs(pts2(i, 2) > FLT_EPSILON)) {
temp_pts1.row(idx) = pts1.row(i);
temp_pts2.row(idx) = pts2.row(i);
idx++;
}
}
pts1 = temp_pts1;
pts2 = temp_pts2;
}
void initTable(ColorCloudPtr cloud) {
MatrixXf corners = getTableCornersRansac(cloud);
Vector3f xax = corners.row(1) - corners.row(0);
xax.normalize();
Vector3f yax = corners.row(3) - corners.row(0);
yax.normalize();
Vector3f zax = xax.cross(yax);
float zsgn = (zax(2) > 0) ? 1 : -1;
xax *= - zsgn;
zax *= - zsgn; // so z axis points up
m_axes.col(0) = xax;
m_axes.col(1) = yax;
m_axes.col(2) = zax;
MatrixXf rotCorners = corners * m_axes;
m_mins = rotCorners.colwise().minCoeff();
m_maxes = rotCorners.colwise().maxCoeff();
m_mins(2) = rotCorners(0,2) + LocalConfig::zClipLow;
m_maxes(2) = rotCorners(0,2) + LocalConfig::zClipHigh;
m_transform.setBasis(btMatrix3x3(
xax(0),yax(0),zax(0),
xax(1),yax(1),zax(1),
xax(2),yax(2),zax(2)));
m_transform.setOrigin(btVector3(corners(0,0), corners(0,1), corners(0,2)));
m_poly.points = toROSPoints32(toBulletVectors(corners));
m_inited = true;
}
IGL_INLINE void igl::embree::ambient_occlusion(
const igl::embree::EmbreeIntersector & ei,
const Eigen::PlainObjectBase<DerivedP> & P,
const Eigen::PlainObjectBase<DerivedN> & N,
const int num_samples,
Eigen::PlainObjectBase<DerivedS> & S)
{
using namespace Eigen;
using namespace igl;
const int n = P.rows();
// Resize output
S.resize(n,1);
// Embree seems to be parallel when constructing but not when tracing rays
#pragma omp parallel for
// loop over mesh vertices
for(int p = 0;p<n;p++)
{
const Vector3f origin = P.row(p).template cast<float>();
const Vector3f normal = N.row(p).template cast<float>();
int num_hits = 0;
MatrixXf D = random_dir_stratified(num_samples).cast<float>();
for(int s = 0;s<num_samples;s++)
{
//Vector3d d = random_dir();
Vector3f d = D.row(s);
if(d.dot(normal) < 0)
{
// reverse ray
d *= -1;
}
igl::embree::Hit hit;
const float tnear = 1e-4f;
if(ei.intersectRay(origin,d,hit,tnear))
{
num_hits++;
}
}
S(p) = (double)num_hits/(double)num_samples;
}
}
void NeighbourJoining::updateD(MatrixXf& D, const MatrixXf& currentD, const Pair& p,
int newNode) {
//calculates distance form all nodes to the new node
D.row(newNode).head(newNode) = (((D.row(p.iID) + D.row(p.jID))
- MatrixXf::Constant(1, D.rows(), 1) * D(p.iID, p.jID)) / 2).head(
newNode);
D.col(newNode).head(newNode) = D.row(newNode).head(newNode);
//calculates distances from child nodes to new node
D(newNode, p.iID) = abs(currentD.row(p.i).head(numCurrentNodes).sum()
- currentD.row(p.j).head(numCurrentNodes).sum());
D(newNode, p.iID) /= (2 * (numCurrentNodes - 2));
D(newNode, p.iID) = D(p.iID, p.jID)/2;
D(p.iID, newNode) = D(newNode, p.iID);
D(newNode, p.jID) = D(p.jID, newNode) = D(p.iID, p.jID) - D(p.iID, newNode);
//cout << "updated D: " << endl << D << endl;
}
// Sample degeneration test, return false if at least three point are colinear
bool sampleValidTest(const MatrixXf &pts1, const MatrixXf &pts2) {
return !(colinearity(pts1.row(1), pts1.row(2), pts1.row(3)) ||
colinearity(pts1.row(0), pts1.row(1), pts1.row(2)) ||
colinearity(pts1.row(0), pts1.row(2), pts1.row(3)) ||
colinearity(pts1.row(0), pts1.row(1), pts1.row(3)) ||
colinearity(pts2.row(1), pts2.row(2), pts2.row(3)) ||
colinearity(pts2.row(0), pts2.row(1), pts2.row(2)) ||
colinearity(pts2.row(0), pts2.row(2), pts2.row(3)) ||
colinearity(pts2.row(0), pts2.row(1), pts2.row(3)));
}
void display()
{
using namespace igl;
using namespace std;
using namespace Eigen;
const float back[4] = {0.75, 0.75, 0.75,0};
glClearColor(back[0],back[1],back[2],0);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
static bool first = true;
if(first)
{
sort();
first = false;
}
if(is_animating)
{
double t = (get_seconds() - animation_start_time)/ANIMATION_DURATION;
if(t > 1)
{
t = 1;
is_animating = false;
}
Quaterniond q = animation_from_quat.slerp(t,animation_to_quat).normalized();
camera.orbit(q.conjugate());
}
glEnable(GL_DEPTH_TEST);
glDepthFunc(GL_LEQUAL);
glEnable(GL_NORMALIZE);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
lights();
push_scene();
// Draw a nice floor
glEnable(GL_DEPTH_TEST);
glPushMatrix();
const double floor_offset =
-2./bbd*(V.col(1).maxCoeff()-Vmid(1));
glTranslated(0,floor_offset,0);
const float GREY[4] = {0.5,0.5,0.6,1.0};
const float DARK_GREY[4] = {0.2,0.2,0.3,1.0};
glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
glEnable(GL_CULL_FACE);
glCullFace(GL_BACK);
draw_floor(GREY,DARK_GREY);
glDisable(GL_CULL_FACE);
glPopMatrix();
push_object();
const auto & draw_skeleton = []()
{
switch(skel_style)
{
default:
case SKEL_STYLE_TYPE_3D:
{
MatrixXf colors = MAYA_VIOLET.transpose().replicate(s.BE.rows(),1);
for(int si=0;si<s.sel.size();si++)
{
for(int b=0;b<s.BE.rows();b++)
{
if(s.BE(b,0) == s.sel(si) || s.BE(b,1) == s.sel(si))
{
colors.row(b) = MAYA_SEA_GREEN;
}
}
}
draw_skeleton_3d(s.C,s.BE,MatrixXd(),colors);
break;
}
case SKEL_STYLE_TYPE_VECTOR_GRAPHICS:
draw_skeleton_vector_graphics(s.C,s.BE);
break;
}
};
if(!skeleton_on_top)
{
draw_skeleton();
}
// Set material properties
glDisable(GL_COLOR_MATERIAL);
glMaterialfv(GL_FRONT, GL_AMBIENT,
Vector4f(GOLD_AMBIENT[0],GOLD_AMBIENT[1],GOLD_AMBIENT[2],alpha).data());
glMaterialfv(GL_FRONT, GL_DIFFUSE,
Vector4f(GOLD_DIFFUSE[0],GOLD_DIFFUSE[1],GOLD_DIFFUSE[2],alpha).data());
glMaterialfv(GL_FRONT, GL_SPECULAR,
Vector4f(GOLD_SPECULAR[0],GOLD_SPECULAR[1],GOLD_SPECULAR[2],alpha).data());
glMaterialf (GL_FRONT, GL_SHININESS, 128);
glMaterialfv(GL_BACK, GL_AMBIENT,
Vector4f(SILVER_AMBIENT[0],SILVER_AMBIENT[1],SILVER_AMBIENT[2],alpha).data());
glMaterialfv(GL_BACK, GL_DIFFUSE,
Vector4f(FAST_GREEN_DIFFUSE[0],FAST_GREEN_DIFFUSE[1],FAST_GREEN_DIFFUSE[2],alpha).data());
glMaterialfv(GL_BACK, GL_SPECULAR,
Vector4f(SILVER_SPECULAR[0],SILVER_SPECULAR[1],SILVER_SPECULAR[2],alpha).data());
glMaterialf (GL_BACK, GL_SHININESS, 128);
if(wireframe)
{
glPolygonMode(GL_FRONT_AND_BACK,GL_LINE);
}
glLineWidth(1.0);
draw_mesh(V,sorted_F,sorted_N);
glPolygonMode(GL_FRONT_AND_BACK,GL_FILL);
if(skeleton_on_top)
{
glDisable(GL_DEPTH_TEST);
draw_skeleton();
}
pop_object();
pop_scene();
report_gl_error();
TwDraw();
glutSwapBuffers();
if(is_animating)
{
glutPostRedisplay();
}
}
IplImage* CloudProjection::computeProjection(const sensor_msgs::PointCloud& data,
const std::vector<int>& interest_region_indices)
{
// -- Put cluster points into matrix form.
MatrixXf points(interest_region_indices.size(), 3);
for(size_t i=0; i<interest_region_indices.size(); ++i) {
points(i, 0) = data.points[interest_region_indices[i]].x;
points(i, 1) = data.points[interest_region_indices[i]].y;
points(i, 2) = data.points[interest_region_indices[i]].z;
}
// -- Subtract off the mean and flatten to z=0 to prepare for PCA.
MatrixXf X = points;
X.col(2) = VectorXf::Zero(X.rows());
VectorXf pt_mean = X.colwise().sum() / (float)X.rows();
for(int i=0; i<X.rows(); ++i) {
X.row(i) -= pt_mean.transpose();
}
MatrixXf Xt = X.transpose();
// -- Find the long axis.
// Start with a random vector.
VectorXf pc = VectorXf::Zero(3);
pc(0) = 1; //Chosen by fair dice roll.
pc(1) = 1;
pc.normalize();
// Power method.
VectorXf prev = pc;
double thresh = 1e-4;
int ctr = 0;
while(true) {
prev = pc;
pc = Xt * (X * pc);
pc.normalize();
ctr++;
if((pc - prev).norm() < thresh)
break;
}
assert(abs(pc(2)) < 1e-4);
// -- Find the short axis.
VectorXf shrt = VectorXf::Zero(3);
shrt(1) = -pc(0);
shrt(0) = pc(1);
assert(abs(shrt.norm() - 1) < 1e-4);
assert(abs(shrt.dot(pc)) < 1e-4);
// -- Build the basis of normalized coordinates.
MatrixXf basis = MatrixXf::Zero(3,3);
basis.col(0) = pc;
basis.col(1) = shrt;
basis(2,2) = -1.0;
assert(abs(basis.col(0).dot(basis.col(1))) < 1e-4);
assert(abs(basis.col(0).norm() - 1) < 1e-4);
assert(abs(basis.col(1).norm() - 1) < 1e-4);
assert(abs(basis.col(2).norm() - 1) < 1e-4);
// -- Put the cluster into normalized coordinates, and choose which axis to project on.
MatrixXf projected_basis(3, 2);
if(axis_ == 0) {
projected_basis.col(0) = basis.col(1);
projected_basis.col(1) = basis.col(2);
}
else if(axis_ == 1) {
projected_basis.col(0) = basis.col(0);
projected_basis.col(1) = basis.col(2);
}
else if(axis_ == 2) {
projected_basis.col(0) = basis.col(0);
projected_basis.col(1) = basis.col(1);
}
MatrixXf projected = points * projected_basis;
// -- Transform into pixel units.
for(int i=0; i<projected.rows(); ++i) {
projected(i, 0) *= pixels_per_meter_;
projected(i, 1) *= pixels_per_meter_;
}
// -- Find min and max of u and v. TODO: noise sensitivity?
float min_v = FLT_MAX;
float min_u = FLT_MAX;
float max_v = -FLT_MAX;
float max_u = -FLT_MAX;
for(int i=0; i<projected.rows(); ++i) {
float u = projected(i, 0);
float v = projected(i, 1);
if(u < min_u)
min_u = u;
if(u > max_u)
max_u = u;
if(v < min_v)
min_v = v;
if(v > max_v)
max_v = v;
}
// -- Shift the origin based on {u,v}_offset_pct.
// u_offset_pct_ is the percent of the way from min_u to max_u that the
// u_offset should be set to. If this makes the window fall outside min_u or max_u,
// then shift the window so that it is inside.
float u_offset = u_offset_pct_ * (max_u - min_u) + min_u;
float v_offset = v_offset_pct_ * (max_v - min_v) + min_v;
if(u_offset_pct_ > 0.5 && u_offset + cols_ / 2 > max_u)
u_offset = max_u - cols_ / 2 + 1;
if(u_offset_pct_ < 0.5 && u_offset - cols_ / 2 < min_u)
u_offset = min_u + cols_ / 2 - 1;
if(v_offset_pct_ > 0.5 && v_offset + rows_ / 2 > max_v)
v_offset = max_v - rows_ / 2 + 1;
if(v_offset_pct_ < 0.5 && v_offset - rows_ / 2 < min_v)
v_offset = min_v + rows_ / 2 - 1;
for(int i=0; i<projected.rows(); ++i) {
projected(i, 0) -= u_offset - (float)cols_ / 2.0;
projected(i, 1) -= v_offset - (float)rows_ / 2.0;
}
// -- Fill the IplImages.
assert(sizeof(float) == 4);
IplImage* acc = cvCreateImage(cvSize(cols_, rows_), IPL_DEPTH_32F, 1);
IplImage* img = cvCreateImage(cvSize(cols_, rows_), IPL_DEPTH_32F, 1);
cvSetZero(acc);
cvSetZero(img);
for(int i=0; i<projected.rows(); ++i) {
int row = floor(projected(i, 1));
int col = floor(projected(i, 0));
if(row >= rows_ || col >= cols_ || row < 0 || col < 0)
continue;
float intensity = (float)data.channels[0].values[interest_region_indices[i]] / 255.0 * (3.0 / 4.0) + 0.25;
//cout << i << ": " << interest_region_indices[i] << "/" << data.channels[0].values.size() << " " << (float)data.channels[0].values[interest_region_indices[i]] << " " << intensity << endl;
assert(interest_region_indices[i] < (int)data.channels[0].values.size() && (int)interest_region_indices[i] >= 0);
assert(intensity <= 1.0 && intensity >= 0.0);
((float*)(img->imageData + row * img->widthStep))[col] += intensity;
((float*)(acc->imageData + row * acc->widthStep))[col]++;
}
// -- Normalize by the number of points falling in each pixel.
for(int v=0; v<rows_; ++v) {
float* img_ptr = (float*)(img->imageData + v * img->widthStep);
float* acc_ptr = (float*)(acc->imageData + v * acc->widthStep);
for(int u=0; u<cols_; ++u) {
if(*acc_ptr == 0)
*img_ptr = 0;
else
*img_ptr = *img_ptr / *acc_ptr;
img_ptr++;
acc_ptr++;
}
}
// -- Clean up and return.
cvReleaseImage(&acc);
return img;
}
int EMclustering::EM(int k, int *IDX, bool spatial, bool att)
{
clusternum = k;
MatrixXf x;
/*if(spatial)
{
if(att)
{
x.resize(4,dataSize);
for(int i=0;i<dataSize;i++)
{
x(0,i) = dataPos[i][0];
x(1,i) = dataPos[i][1];
x(2,i) = dataPos[i][2];
x(3,i) = dataDen[i];
}
}
else
{
x.resize(6,dataSize);
for(int i=0;i<dataSize;i++)
{
x(0,i) = dataPos[i][0];
x(1,i) = dataPos[i][1];
x(2,i) = dataPos[i][2];
x(3,i) = dataVel[i][0];
x(4,i) = dataVel[i][1];
x(5,i) = dataVel[i][2];
}
}
}
else
{*/
if(att)
{
x.resize(1,dataDen.size());
for(int i=0;i<dataDen.size();i++)
{
x(0,i) = dataDen[i];
}
//cerr<<x;
//cerr<<endl;
if(k>dataDen.size())
return -1;
}
else
{
x.resize(3,dataSize);
for(int i=0;i<dataSize;i++)
{
x(0,i) = dataVel[i][0];//fabs(cos(-PI/4)*dataVel[i][0] - sin(-PI/4)*dataVel[i][1]);
x(1,i) = dataVel[i][1];//fabs(sin(-PI/4)*dataVel[i][0] + cos(-PI/4)*dataVel[i][1]);
x(2,i) = dataVel[i][2];
}
if(k>dataSize)
return -1;
}
//}
//cout<<"EM for Gaussian mixture: running ... "<<endl;
//cerr<<x<<endl;
MatrixXf r =initialization(x,k);// kmeans(x,k);//
//cerr<<"Initialization is Done"<<endl;//cerr<<r<<endl;
VectorXi label(r.rows());
for(int i=0;i<r.rows();i++)
{
int index;
float tmp1 = r.row(i).maxCoeff(&index);
label(i) = index;
}//cerr<<label<<endl;
VectorXi tmpp(label.size());
VectorXi tmp2 = unique(label,tmpp);
int tmpd = tmp2.size(); //cerr<<tmpd<<endl;
MatrixXf tmpr(r.rows(),tmpd);
for(int i=0;i<tmpd;i++)
{
tmpr.col(i) = r.col(tmp2(i));
}//cerr<<"done1"<<endl;
r.resize(r.rows(),tmpd);
r = tmpr;//cerr<<r.cols()<<endl;
float tol = 1e-10;
int max = 300;
double llh = -9e+9;
bool converged = false;
int t = 1;
//cerr<<"done1"<<endl;
//gaussian_model model;
int clusternum_error;
MatrixXf tmpmodel;
while(!converged&&t<max)
{
t = t + 1;
gaussian_model model = maximization(x,r);//cerr<<t<<" "<<"max"<<endl;
float tmpllh = llh;
r = expectation(x,model,llh);//cerr<<t<<" "<<"exp"<<endl;
for(int i=0;i<r.rows();i++)
{
int index;
float tmp1 = r.row(i).maxCoeff(&index);
label(i) = index;
}
VectorXi u = unique(label,tmpp);//cerr<<t<<" "<<u.size()<<" "<<r.cols()<<" "<<r.rows()<<endl;
clusternum_error = clusternum - u.size();
if(r.cols()!=u.size())
{
/* tmpr.resize(r.rows(),u.size());
for(int i=0;i<u.size();i++)
{
tmpr.col(i) = r.col(u(i));
}
r.resize(r.rows(),u.size());
r = tmpr;//cerr<<"r"<<endl;*/
}
else
{
if((llh - tmpllh)<tol*abs(llh))
converged = true;
else
converged = false;
}
//cerr<<"t"<<t<<endl;
//return_model = model;
tmpmodel.resize(model.mu.rows(),model.mu.cols());
//return_model = model.mu;
tmpmodel = model.mu;
u.resize(0);
//cerr<<tmpmodel<<endl;
}
/*ofstream off2("rr");
off2<<r.row(0)<<endl;
for(int i=1;i<r.rows();i++)
if(r.row(i)!=r.row(i-1))
{off2<<x.col(i)<<" ";
off2<<r.row(i)<<endl;}
off2.close();*/
cerr<<clusternum_error<<endl;
return_model = tmpmodel;
//cerr<<label<<endl;
if (converged)
cerr<<"Converged in "<<t-1<<endl;
else
cerr<<max<<endl;
//cerr<<t-1<<endl;
for(int i=0;i<label.size();i++)
{
IDX[i] = label(i);
//cerr<<IDX[i]<<" ";
}//cerr<<endl;
//cerr<<label.size()<<endl;
x.resize(0,0);
r.resize(0,0);
tmpr.resize(0,0);
tmpmodel.resize(0,0);
label.resize(0);
tmpp.resize(0);
tmp2.resize(0);
return clusternum_error;
}
void TMSI::run()
{
while(m_bIsRunning)
{
//std::cout<<"TMSI::run(s)"<<std::endl;
//pop matrix only if the producer thread is running
if(m_pTMSIProducer->isRunning())
{
MatrixXf matValue = m_pRawMatrixBuffer_In->pop();
// Set Beep trigger (if activated)
if(m_bBeepTrigger && m_qTimerTrigger.elapsed() >= m_iTriggerInterval)
{
QFuture<void> future = QtConcurrent::run(Beep, 450, 700);
//Set trigger in received data samples - just for one sample, so that this event is easy to detect
matValue(136, m_iSamplesPerBlock-1) = 252;
m_qTimerTrigger.restart();
Q_UNUSED(future);
}
// Set keyboard trigger (if activated and !=0)
if(m_bUseKeyboardTrigger && m_iTriggerType!=0)
matValue(136, m_iSamplesPerBlock-1) = m_iTriggerType;
//Write raw data to fif file
if(m_bWriteToFile)
m_pOutfid->write_raw_buffer(matValue.cast<double>(), m_cals);
// TODO: Use preprocessing if wanted by the user
if(m_bUseFiltering)
{
MatrixXf temp = matValue;
matValue = matValue - m_matOldMatrix;
m_matOldMatrix = temp;
// //Check filter class - will be removed in the future - testing purpose only!
// FilterTools* filterObject = new FilterTools();
// //kaiser window testing
// qint32 numberCoeff = 51;
// QVector<float> impulseResponse(numberCoeff);
// filterObject->createDynamicFilter(QString('LP'), numberCoeff, (float)0.3, impulseResponse);
// ofstream outputFileStream("mne_x_plugins/resources/tmsi/filterToolsTest.txt", ios::out);
// outputFileStream << "impulseResponse:\n";
// for(int i=0; i<impulseResponse.size(); i++)
// outputFileStream << impulseResponse[i] << " ";
// outputFileStream << endl;
// //convolution testing
// QVector<float> in (12, 2);
// QVector<float> kernel (4, 2);
// QVector<float> out = filterObject->convolve(in, kernel);
// outputFileStream << "convolution result:\n";
// for(int i=0; i<out.size(); i++)
// outputFileStream << out[i] << " ";
// outputFileStream << endl;
}
// TODO: Perform a fft if wanted by the user
if(m_bUseFFT)
{
QElapsedTimer timer;
timer.start();
FFT<float> fft;
Matrix<complex<float>, 138, 16> freq;
for(qint32 i = 0; i < matValue.rows(); ++i)
fft.fwd(freq.row(i), matValue.row(i));
// cout<<"FFT postprocessing done in "<<timer.nsecsElapsed()<<" nanosec"<<endl;
// cout<<"matValue before FFT:"<<endl<<matValue<<endl;
// cout<<"freq after FFT:"<<endl<<freq<<endl;
// matValue = freq.cwiseAbs();
// cout<<"matValue after FFT:"<<endl<<matValue<<endl;
}
//Change values of the trigger channel for better plotting - this change is not saved in the produced fif file
if(m_iNumberOfChannels>137)
{
for(int i = 0; i<matValue.row(137).cols(); i++)
{
// Left keyboard or capacitive
if(matValue.row(136)[i] == 254)
matValue.row(136)[i] = 4000;
// Right keyboard
if(matValue.row(136)[i] == 253)
matValue.row(136)[i] = 8000;
// Beep
if(matValue.row(136)[i] == 252)
matValue.row(136)[i] = 2000;
}
}
//emit values to real time multi sample array
for(qint32 i = 0; i < matValue.cols(); ++i)
m_pRMTSA_TMSI->data()->setValue(matValue.col(i).cast<double>());
// Reset keyboard trigger
m_iTriggerType = 0;
}
}
//Close the fif output stream
if(m_bWriteToFile)
m_pOutfid->finish_writing_raw();
//std::cout<<"EXITING - TMSI::run()"<<std::endl;
}
bool multiModelRANSAC(const MatrixXf &data, int M, MatrixXf &inlier) {
int maxdegen = 10;
int dataSize = data.rows();
int psize = 4;
int blockSize = 10;
MatrixXf x1 = data.block(0, 0, data.rows(), 3);
MatrixXf x2 = data.block(0, 3, data.rows(), 3);
vector<int> sample;
MatrixXf pts1(4, 3);
MatrixXf pts2(4, 3);
int h = 0;
MatrixXf Hs(M, 9);
MatrixXf inx(M, psize);
MatrixXf res(dataSize, M);
MatrixXi resIndex(dataSize, M);
for (int m = 0; m < M; m++) {
int degencount = 0;
int isdegen = 1;
while (isdegen==1 && degencount < maxdegen) {
degencount++;
if (m < blockSize)
RandomSampling(psize, dataSize, sample);
else
WeightedSampling(psize, dataSize, resIndex, sample, h);
for (int i = 0; i < psize; i++) {
pts1.row(i) = x1.row(sample[i]);
pts2.row(i) = x2.row(sample[i]);
}
if (sampleValidTest(pts1, pts2))
isdegen = 0;
}
if (isdegen) {
cout << "Cannot find valid p-subset" << endl;
return false;
}
for (int i = 0; i < psize; i++)
inx(m, i) = sample[i];
Matrix3f temp_H;
MatrixXf temp_A, localResidue;
fitHomography(pts1, pts2, temp_H, temp_A);
computeHomographyResidue(x1, x2, temp_H, localResidue);
Hs.row(m) = unrollMatrix3f(temp_H);
res.col(m) = localResidue;
if (m >= (blockSize-1) && (m+1)%blockSize == 0) {
h = round(0.1f*m);
sortResidueForIndex(res, (m/blockSize)*blockSize, ((m+1)/blockSize)*blockSize, resIndex);
}
}
VectorXf bestModel(M);
bestModel.setZero();
int bestIndex = 0;
int bestCount = -1;
for (int i = 0; i < M; i++) {
for (int j = 0; j < dataSize; j++)
if (res(j, i) < THRESHOLD)
bestModel(i) += 1;
if (bestModel(i) > bestCount) {
bestIndex = i;
bestCount = bestModel(i);
}
}
VectorXf bestModelRes = res.col(bestIndex);
int inlierCount = (bestModelRes.array() < THRESHOLD).count();
inlier.resize(inlierCount, data.cols());
int runningIdx = 0;
for (int i = 0; i < dataSize; i++)
if (bestModelRes(i) < THRESHOLD) {
inlier.row(runningIdx) = data.row(i);
runningIdx ++;
}
return true;
}
MixGaussProb::MixGaussProb(
const MatrixXf& data,
const MultiD& mu,
const MultiD& Sigma,
const MatrixXf& _mixmat
) {
MatrixXf mixmat = _mixmat;
/*
if iscolumn(mu)
d = length(mu);
Q = 1; M = 1;
elseif ismatrix(mu)
[~, Q] = size(mu);
M = 1;
else
[~, Q M] = size(mu);
end*/
//we always have 3D mu
size_t Q = mu(0,0).cols(); // not safe
size_t M = mu.rows();
size_t T = data.cols(); //[d T] = size(data);
if(mixmat.rows() == 0 && mixmat.cols() == 0) { // if nargin < 4, mixmat = ones(Q,1); end
mixmat = MatrixXf::Ones(Q, 1);
}
//sigma always 4d, so implement % general case
B2.resize(T,1); //B2 = zeros(Q,M,T);
for(size_t i = 0; i < T; ++i) {
B2(i, 0) = MatrixXf::Zero(Q, M);
}
#ifdef DEBUG_GAUSSIAN_PROB
print4DMatrix(mu);
#endif
for(size_t j = 0; j < Q; ++j) { //for j=1:Q
for(size_t k = 0; k < M; ++k) { //for k=1:M
MatrixXf sig = Sigma(j, k);
MatrixXf m = mu(k, 0).col(j);
MatrixXf t = gaussianProb(data, m, sig);
for(size_t index = 0; index < (size_t)t.rows(); ++index) {
B2(index, 0)(j, k) = t(index, 0); //B2(j,k,:) = gaussian_prob(data, mu(:,j,k), Sigma(:,:,j,k));
}
}
}
B = MatrixXf::Zero(Q,T);//B = zeros(Q,T);
if(Q < T) {
for(size_t q = 0; q < Q; ++q) {
MatrixXf tmp(M, T);
for(size_t i = 0; i < T; ++i) {
MatrixXf B2i1 = B2(i,0);
tmp.col(i) = B2(i,0).row(q);
} //permute(B2(q,:,:), [2 3 1])
B.row(q) = mixmat.row(q) * tmp;
// B(q,:) = mixmat(q,:) * permute(B2(q,:,:), [2 3 1]); % vector * matrix sums over m
}
}
else {
std::string unimplemented_code = "for t=1:T\nB(:,t) = sum(mixmat .* B2(:,:,t), 2); % sum over m\nend\n";
throw std::runtime_error(unimplemented_code);
}
#ifdef DEBUG_MIX_GAUSS_PROB
std::cout << B << std::endl;
print4DMatrix(B2);
#endif
}
