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);
}
void sumAndNormalize( MatrixXf & out, const MatrixXf & in, const MatrixXf & Q ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<in.cols(); i++ ){
VectorXf b = in.col(i);
VectorXf q = Q.col(i);
out.col(i) = b.array().sum()*q - b;
}
}
///////////////////////
///// Inference /////
///////////////////////
void expAndNormalize ( MatrixXf & out, const MatrixXf & in ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<out.cols(); i++ ){
VectorXf b = in.col(i);
b.array() -= b.maxCoeff();
b = b.array().exp();
out.col(i) = b / b.array().sum();
}
}
void noHomogeneous(MatrixXf &mat) {
MatrixXf temp;
if (mat.cols() == 3) {
temp.resize(mat.rows(), 2);
temp.col(0).array() = mat.col(0).array()/mat.col(2).array();
temp.col(1).array() = mat.col(1).array()/mat.col(2).array();
mat = temp;
} else
cout << "toHomogeneous with wrong dimension" << endl;
}
//update values. Order matters
void SoftConstraint::updateDefGrad() {
this->D_s.col(0) << p0->position - p3->position;
this->D_s.col(1) << p1->position - p3->position;
this->D_s.col(2) << p2->position - p3->position;
this->defGrad = D_s * D_m.inverse();
float det = defGrad.determinant();
//check if cell is degenerated, i.e. det(defgrad) <= 0)
if(det<= 0) {
this->degenrated = true;
//compute singular value decomposition of defGrad
JacobiSVD<_Matrix3> svd(this->defGrad, ComputeFullU | ComputeFullV);
MatrixXf F_sing = svd.singularValues();
MatrixXf U = svd.matrixU();
MatrixXf V = svd.matrixV();
this->U = U;
this->V = V;
//find smalles Element and negate corresponding colums
int smallestEntry = 0;
if(F_sing(1) < F_sing(0)) {
smallestEntry = 1;
}
if(F_sing(2) < F_sing(smallestEntry)) {
smallestEntry = 2;
}
F_sing(smallestEntry) = - F_sing(smallestEntry);
U.col(smallestEntry) = -U.col(smallestEntry);
//compose new defGrad
_Matrix3 F;
F << F_sing(0), 0.0f, 0.0f,
0.0f, F_sing(1), 0.0f,
0.0f, 0.0f, F_sing(2);
this->defGrad = F;
//cout << "det <0";
}
else {
this->degenrated = false;
}
};
MatrixXf Arm::compute_Jacobian() {
/*X, Y, Z are three different orthogonal axises on which a ball joint can rotate. */
MatrixXf Jacobian = MatrixXf(3, 3 * arm_sequence.size() );
for (int i = 0; i < arm_sequence.size(); i++) {
Vector3f dPdT_joint_i_X = dPdT(i, X);
Vector3f dPdT_joint_i_Y = dPdT(i, Y);
Vector3f dPdT_joint_i_Z = dPdT(i, Z);
Jacobian.col(3 * i) << dPdT_joint_i_X;
Jacobian.col(3 * i + 1) << dPdT_joint_i_Y;
Jacobian.col(3 * i + 2) << dPdT_joint_i_Z;
}
//cout << "Jacobian \n" << Jacobian << endl;
return Jacobian;
}
void operator()() {
#ifdef FAST_AND_FAT
//! Performing feature transformation on a block results
//! is about a factor of two speedup. This suggests moving
//! feature storage from nodes to images. However, the
//! change will make introduction of different node
//! types (with different sized feature vectors) difficult.
//! It also means that the metric functors would need to
//! be passed a copy of the graph in addition to the nodes.
for (set<unsigned>::const_iterator it = _imgIndxes.begin(); it != _imgIndxes.end(); ++it) {
lock();
map<unsigned, MatrixXf>::const_iterator ft = _imgFeatureData.find(*it);
if (ft == _imgFeatureData.end()) {
MatrixXf X(_Lt.cols(), _srcGraph[*it].numNodes());
for (unsigned segId = 0; segId < _srcGraph[*it].numNodes(); segId++) {
X.col(segId) = _srcGraph[*it][segId].features;
}
ft = _imgFeatureData.insert(_imgFeatureData.end(), make_pair(*it, X));
}
unlock();
const MatrixXf X = _Lt.triangularView<Eigen::Upper>() * ft->second;
for (unsigned segId = 0; segId < _srcGraph[*it].numNodes(); segId++) {
_negGraph[*it][segId].features = _posGraph[*it][segId].features = X.col(segId);
}
}
#else
const TriangularView<MatrixXf, Eigen::Upper> Lt(_Lt);
for (set<unsigned>::const_iterator it = _imgIndxes.begin(); it != _imgIndxes.end(); ++it) {
for (unsigned segId = 0; segId < _srcGraph[*it].numNodes(); segId++) {
_negGraph[*it][segId].features = _posGraph[*it][segId].features = Lt * _srcGraph[*it][segId].features;
}
}
#endif
}
/**
* Normalizes each eigenface in a matrix.
*
* @param eigenfaces A matrix of eigen faces to normalize
*/
void normalizeEigenFaces(MatrixXf &eigenfaces)
{
for(int i = 0; i < eigenfaces.cols(); i++)
{
eigenfaces.col(i).normalize();
}
}
void RealtimeMF_openni::projectDirections(cv::Mat& I, const MatrixXf& dirs,
double f_d, const Matrix<uint8_t,Dynamic,Dynamic>& colors)
{
double scale = 0.1;
VectorXf p0(3); p0 << 0.35,0.25,1;
double u0 = p0(0)/p0(2)*f_d + 320.;
double v0 = p0(1)/p0(2)*f_d + 240.;
for(uint32_t k=0; k < dirs.cols(); ++k)
{
VectorXf p1 = p0 + dirs.col(k)*scale;
double u1 = p1(0)/p1(2)*f_d + 320.;
double v1 = p1(1)/p1(2)*f_d + 240.;
cv::line(I, cv::Point(u0,v0), cv::Point(u1,v1),
CV_RGB(colors(k,0),colors(k,1),colors(k,2)), 2, CV_AA);
double arrowLen = 10.;
double angle = atan2(v1-v0,u1-u0);
double ru1 = u1 - arrowLen*cos(angle + M_PI*0.25);
double rv1 = v1 - arrowLen*sin(angle + M_PI*0.25);
cv::line(I, cv::Point(u1,v1), cv::Point(ru1,rv1),
CV_RGB(colors(k,0),colors(k,1),colors(k,2)), 2, CV_AA);
ru1 = u1 - arrowLen*cos(angle - M_PI*0.25);
rv1 = v1 - arrowLen*sin(angle - M_PI*0.25);
cv::line(I, cv::Point(u1,v1), cv::Point(ru1,rv1),
CV_RGB(colors(k,0),colors(k,1),colors(k,2)), 2, CV_AA);
}
cv::circle(I, cv::Point(u0,v0), 2, CV_RGB(0,0,0), 2, CV_AA);
}
static inline MatrixXf toMatrix(const std::vector<Vector3f> &data) {
MatrixXf result;
result.resize(3, data.size());
for (size_t i = 0; i < data.size(); ++i) {
result.col(i) = data[i];
}
return std::move(result);
}
void toHomogeneous(MatrixXf &mat) {
MatrixXf temp;
if (mat.cols() == 2) {
temp.resize(mat.rows(), 3);
temp.leftCols<2>() = mat.leftCols<2>();
temp.col(2).setConstant(1);
mat = temp;
} else if (mat.cols() == 4) {
temp.resize(mat.rows(), 6);
temp.leftCols<2>() = mat.leftCols<2>();
temp.col(2).setConstant(1);
temp.block(0, 3, mat.rows(), 2) = temp.block(0, 2, mat.rows(), 2);
temp.col(5).setConstant(1);
mat = temp;
} else
cout << "toHomogeneous with wrong dimension" << endl;
}
MatrixXf RealtimeMF::mfAxes()
{
MatrixXf mfAx = MatrixXf::Zero(3,6*cRmfs_.size());
for(uint32_t k=0; k<6*cRmfs_.size(); ++k){
int j = (k%6)/2; // which of the rotation columns does this belong to
float sign = (- float((k%6)%2) +0.5f)*2.0f; // sign of the axis
mfAx.col(k) = sign*cRmfs_[k/6].col(j);
}
return mfAx;
};
Vector3f Lu::estimate_t( MatrixXf R,MatrixXf G,std::vector<Matrix3f > F,MatrixXf P,int n ){
Vector3f sum_ =Vector3f::Zero();
for (int i=0;i< n; i++){
sum_=sum_+F[i]*R*P.col(i);
}
return G*sum_;
}
VectorXs DenseCRF::currentMap( const MatrixXf & Q ) const{
VectorXs r(Q.cols());
// Find the map
for( int i=0; i<N_; i++ ){
int m;
Q.col(i).maxCoeff( &m );
r[i] = m;
}
return r;
}
void RealtimeMF_openni::normals_cb(float* d_normals, uint8_t* haveData,
uint32_t w, uint32_t h)
{
tLog_.tic(-1); // reset all timers
int32_t nComp = 0;
float* d_nComp = this->normalExtract->d_normalsComp(nComp);
Matrix3f kRwBefore = kRw_;
tLog_.toctic(0,1);
optSO3_->updateExternalGpuNormals(d_nComp,nComp,3,0);
double residual = optSO3_->conjugateGradientCUDA(kRw_,nCGIter_);
double D_KL = optSO3_->D_KL_axisUnif();
tLog_.toctic(1,2);
{
boost::mutex::scoped_lock updateLock(this->updateModelMutex);
this->normalsImg_ = this->normalExtract->normalsImg();
if(z_.rows() != w*h) z_.resize(w*h);
this->normalExtract->uncompressCpu(optSO3_->z().data(), optSO3_->z().rows(),
z_.data(), z_.rows());
mfAxes_ = MatrixXf::Zero(3,6);
for(uint32_t k=0; k<6; ++k){
int j = k/2; // which of the rotation columns does this belong to
float sign = (- float(k%2) +0.5f)*2.0f; // sign of the axis
mfAxes_.col(k) = sign*kRw_.col(j);
}
D_KL_= D_KL;
residual_ = residual;
this->update_ = true;
updateLock.unlock();
}
tLog_.toc(2); // total time
tLog_.logCycle();
cout<<"delta rotation kRw_ = \n"<<kRwBefore*kRw_.transpose()<<endl;
cout<<"---------------------------------------------------------------------------"<<endl;
tLog_.printStats();
cout<<" residual="<<residual_<<"\t D_KL="<<D_KL_<<endl;
cout<<"---------------------------------------------------------------------------"<<endl;
fout_<<D_KL_<<" "<<residual_<<endl; fout_.flush();
//// return kRw_;
// {
// boost::mutex::scoped_lock updateLock(this->updateModelMutex);
//// pcl::PointCloud<pcl::PointXYZRGB>::ConstPtr nDispPtr =
//// normalExtract->normalsPc();
//// nDisp_ = pcl::PointCloud<pcl::PointXYZRGB>::Ptr( new
//// pcl::PointCloud<pcl::PointXYZRGB>(*nDispPtr));
//// this->normalsImg_ = this->normalExtract->normalsImg();
// this->update_ = true;
// }
};
VectorXf project1D( const RMatrixXf & Y, int * rep_label=NULL ) {
// const int MAX_SAMPLE = 20000;
const bool fast = true, very_fast = true;
// Remove the DC (Y : N x M)
RMatrixXf dY = Y.rowwise() - Y.colwise().mean();
// RMatrixXf sY = dY;
// if( 0 < MAX_SAMPLE && MAX_SAMPLE < dY.rows() ) {
// VectorXi samples = randomChoose( dY.rows(), MAX_SAMPLE );
// std::sort( samples.data(), samples.data()+samples.size() );
// sY = RMatrixXf( samples.size(), dY.cols() );
// for( int i=0; i<samples.size(); i++ )
// sY.row(i) = dY.row( samples[i] );
// }
// ... and use (pc > 0)
VectorXf lbl = VectorXf::Zero( Y.rows() );
// Find the largest PC of (dY.T * dY) and project onto it
if( very_fast ) {
// Find the largest PC using poweriterations
VectorXf U = VectorXf::Random( dY.cols() );
U = U.array() / U.norm()+std::numeric_limits<float>::min();
for( int it=0; it<20; it++ ){
// Normalize
VectorXf s = dY.transpose()*(dY*U);
s.array() /= s.norm()+std::numeric_limits<float>::min();
if ( (U-s).norm() < 1e-6 )
break;
U = s;
}
// Project onto the PC
lbl = dY*U;
}
else if(fast) {
// Compute the eigen values of the covariance (and project onto the largest eigenvector)
MatrixXf cov = dY.transpose()*dY;
SelfAdjointEigenSolver<MatrixXf> eigensolver(0.5*(cov+cov.transpose()));
MatrixXf ev = eigensolver.eigenvectors();
lbl = dY * ev.col( ev.cols()-1 );
}
else {
// Use the SVD
JacobiSVD<RMatrixXf> svd = dY.jacobiSvd(ComputeThinU | ComputeThinV );
// Project onto the largest PC
lbl = svd.matrixU().col(0) * svd.singularValues()[0];
}
// Find the representative label
if( rep_label )
dY.array().square().rowwise().sum().minCoeff( rep_label );
return (lbl.array() < 0).cast<float>();
}
MatrixXf Lu::xform(MatrixXf P, Matrix3f R, Vector3f t){
int n = P.cols();
MatrixXf Q=MatrixXf::Zero(3,n);
for (int i=0;i<n;i++){
Q.col(i) = R*P.col(i)+t;
}
//std::cout<<"Q\n"<<Q<<std::endl;
return Q;
}
/**
* Create a new matrix which is matrix A rearranged.
* This matrix has the same size as B
*/
inline MatrixXf rearrangeMatrixToClosestPoints(const MatrixXf A, const MatrixXf B) {
MatrixXf result = MatrixXf::Constant(B.rows(), B.cols(), 0);
// For each point in B, find the closest point in A
for(uint b = 0; b < B.cols(); b++) {
Vector3f pointInB = B.col(b);
float minDistance = std::numeric_limits<float>::max();
uint closestPoint = 0;
for(uint a = 0; a < A.cols(); a++) {
Vector3f pointInA = A.col(a);
float distance = (pointInA-pointInB).norm();
if(distance < minDistance) {
minDistance = distance;
closestPoint = a;
}
}
result.col(b) = A.col(closestPoint);
}
return result;
}
// XFORMPROJ - Transform and project
// XFORMPROJ(P, R, t) transform the 3D point set P by
// rotation R and translation t, and then project them to the normalized image plane
MatrixXf Lu::xformproj(MatrixXf P, Matrix3f R, Vector3f t)
{
int n = P.cols();
MatrixXf Q=MatrixXf::Zero(3,n);
MatrixXf Qp=MatrixXf::Zero(2,n);;
for (int i=0;i<=n;i++){
Q.col(i) = R*P.col(i)+t;
Qp.col(i)=Q.col(i)/Q(3,i);
}
}
void normalizePts(MatrixXf &mat, Matrix3f &T) {
float cx = mat.col(0).mean();
float cy = mat.col(1).mean();
mat.array().col(0) -= cx;
mat.array().col(1) -= cy;
float sqrt_2 = sqrt(2);
float meandist = (mat.array().col(0)*mat.array().col(0) + mat.array().col(1)*mat.array().col(1)).sqrt().mean();
float scale = sqrt_2/meandist;
mat.leftCols<2>().array() *= scale;
T << scale, 0, -scale*cx, 0, scale, -scale*cy, 0, 0, 1;
}
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);
}
void MneRtClient::run()
{
MatrixXf matValue;
while(true)
{
//pop matrix
matValue = m_pRawMatrixBuffer_In->pop();
// std::cout << "matValue " << matValue.block(0,0,1,10) << std::endl;
//emit values
for(qint32 i = 0; i < matValue.cols(); ++i)
m_pRTMSA_MneRtClient->setValue(matValue.col(i).cast<double>());
// for(qint32 i = 0; i < matValue.cols(); i += 100)
// m_pRTMSA_MneRtClient->setValue(matValue.col(i).cast<double>());
}
}
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;
}
void Util::processGramSchmidt(MatrixXf& mat){
for (int i = 0; i < mat.cols(); ++i){
for (int j = 0; j < i; ++j){
float r = mat.col(i).dot(mat.col(j));
mat.col(i) -= r * mat.col(j);
}
float norm = mat.col(i).norm();
if (norm < SVD_EPS){
for (int k = i; k < mat.cols(); ++k){
mat.col(k).setZero();
}
return;
}
mat.col(i) *= (1.f / norm);
}
}
void FiffSimulator::run()
{
MatrixXf matValue;
while(true)
{
{
QMutexLocker locker(&m_qMutex);
if(!m_bIsRunning)
break;
}
//pop matrix
matValue = m_pRawMatrixBuffer_In->pop();
// std::cout << "Mat Value\n" << matValue.row(306) << std::endl;
//emit values
for(qint32 i = 0; i < matValue.cols(); ++i)
m_pRTMSA_FiffSimulator->data()->setValue(matValue.col(i).cast<double>());
}
}
inline
void r_and_t(MatrixXf &rot_cw, VectorXf &pos_cw,MatrixXf start_points, MatrixXf end_points,
MatrixXf P1w,MatrixXf P2w,MatrixXf initRot_cw,VectorXf initPos_cw,
int maxIterNum,float TerminateTh,int nargin)
{
if(nargin<6)
{
return;
}
if(nargin<8)
{
maxIterNum = 8;
TerminateTh = 1e-5;
}
int n = start_points.cols();
if(n != end_points.cols() || n!= P1w.cols() || n!= P2w.cols())
{
return;
}
if(n<4)
{
return;
}
//first compute the weight of each line and the normal of
//the interpretation plane passing through to camera center and the line
VectorXf w = VectorXf::Zero(n);
MatrixXf nc = MatrixXf::Zero(3,n);
for(int i = 0 ; i < n ; i++)
{
//the weight of a line is the inverse of its image length
w[i] = 1/(start_points.col(i)-end_points.col(i)).norm();
vfloat3 v1 = start_points.col(i);
vfloat3 v2 = end_points.col(i);
vfloat3 temp = v1.cross(v2);
nc.col(i) = temp/temp.norm();
}
MatrixXf rot_wc = initPos_cw.transpose();
MatrixXf pos_wc = - initRot_cw.transpose() * initPos_cw;
for(int iter = 1 ; iter < maxIterNum ; iter++)
{
//construct the equation (31)
MatrixXf A = MatrixXf::Zero(6,7);
MatrixXf C = MatrixXf::Zero(3,3);
MatrixXf D = MatrixXf::Zero(3,3);
MatrixXf F = MatrixXf::Zero(3,3);
vfloat3 c_bar = vfloat3(0,0,0);
vfloat3 d_bar = vfloat3(0,0,0);
for(int i = 0 ; i < n ; i++)
{
//for first point on line
vfloat3 Pi = rot_wc * P1w.col(i);
vfloat3 Ni = nc.col(i);
float wi = w[i];
vfloat3 bi = Pi.cross(Ni);
C = C + wi*Ni*Ni.transpose();
D = D + wi*bi*bi.transpose();
F = F + wi*Ni*bi.transpose();
vfloat3 tempi = Pi + pos_wc;
float scale = Ni.transpose() * tempi;
scale *= wi;
c_bar = c_bar + scale * Ni;
d_bar = d_bar + scale*bi;
//for second point on line
Pi = rot_wc * P2w.col(i);
Ni = nc.col(i);
wi = w[i];
bi = Pi.cross(Ni);
C = C + wi*Ni*Ni.transpose();
D = D + wi*bi*bi.transpose();
F = F + wi*Ni*bi.transpose();
scale = (Ni.transpose() * (Pi + pos_wc));
scale *= wi;
c_bar = c_bar + scale * Ni;
d_bar = d_bar + scale * bi;
}
A.block<3,3>(0,0) = C;
A.block<3,3>(0,3) = F;
(A.col(6)).segment(0,2) = c_bar;
A.block<3,3>(3,0) = F.transpose();
A.block<3,3>(2,2) = D;
(A.col(6)).segment(3,5) = d_bar;
//sovle the system by using SVD;
JacobiSVD<MatrixXf> svd(A, ComputeThinU | ComputeThinV);
VectorXf vec(7);
//the last column of Vmat;
vec = (svd.matrixV()).col(6);
//the condition that the last element of vec should be 1.
vec = vec/vec[6];
//update the rotation and translation parameters;
vfloat3 dT = vec.segment(0,2);
vfloat3 dOmiga = vec.segment(3,5);
MatrixXf rtemp(3,3);
rtemp << 1, -dOmiga[2], dOmiga[1], dOmiga[2], 1, -dOmiga[1], -dOmiga[1], dOmiga[0], 1;
rot_wc = rtemp * rot_wc;
//newRot_wc = ( I + [dOmiga]x ) oldRot_wc
//may be we can compute new R using rodrigues(r+dr)
pos_wc = pos_wc + dT;
if(dT.norm() < TerminateTh && dOmiga.norm() < 0.1*TerminateTh)
{
break;
}
}
rot_cw = rot_wc.transpose();
pos_cw = -rot_cw * pos_wc;
}
void IterativeClosestPoint::execute() {
float error = std::numeric_limits<float>::max(), previousError;
uint iterations = 0;
// Get access to the two point sets
PointSetAccess::pointer accessFixedSet = ((PointSet::pointer)getStaticInputData<PointSet>(0))->getAccess(ACCESS_READ);
PointSetAccess::pointer accessMovingSet = ((PointSet::pointer)getStaticInputData<PointSet>(1))->getAccess(ACCESS_READ);
// Get transformations of point sets
AffineTransformation::pointer fixedPointTransform2 = SceneGraph::getAffineTransformationFromData(getStaticInputData<PointSet>(0));
Eigen::Affine3f fixedPointTransform;
fixedPointTransform.matrix() = fixedPointTransform2->matrix();
AffineTransformation::pointer initialMovingTransform2 = SceneGraph::getAffineTransformationFromData(getStaticInputData<PointSet>(1));
Eigen::Affine3f initialMovingTransform;
initialMovingTransform.matrix() = initialMovingTransform2->matrix();
// These matrices are Nx3
MatrixXf fixedPoints = accessFixedSet->getPointSetAsMatrix();
MatrixXf movingPoints = accessMovingSet->getPointSetAsMatrix();
Eigen::Affine3f currentTransformation = Eigen::Affine3f::Identity();
// Want to choose the smallest one as moving
bool invertTransform = false;
if(false && fixedPoints.cols() < movingPoints.cols()) {
reportInfo() << "switching fixed and moving" << Reporter::end;
// Switch fixed and moving
MatrixXf temp = fixedPoints;
fixedPoints = movingPoints;
movingPoints = temp;
invertTransform = true;
// Apply initial transformations
//currentTransformation = fixedPointTransform.getTransform();
movingPoints = fixedPointTransform*movingPoints.colwise().homogeneous();
fixedPoints = initialMovingTransform*fixedPoints.colwise().homogeneous();
} else {
// Apply initial transformations
//currentTransformation = initialMovingTransform.getTransform();
movingPoints = initialMovingTransform*movingPoints.colwise().homogeneous();
fixedPoints = fixedPointTransform*fixedPoints.colwise().homogeneous();
}
do {
previousError = error;
MatrixXf movedPoints = currentTransformation*(movingPoints.colwise().homogeneous());
// Match closest points using current transformation
MatrixXf rearrangedFixedPoints = rearrangeMatrixToClosestPoints(
fixedPoints, movedPoints);
// Get centroids
Vector3f centroidFixed = getCentroid(rearrangedFixedPoints);
//reportInfo() << "Centroid fixed: " << Reporter::end;
//reportInfo() << centroidFixed << Reporter::end;
Vector3f centroidMoving = getCentroid(movedPoints);
//reportInfo() << "Centroid moving: " << Reporter::end;
//reportInfo() << centroidMoving << Reporter::end;
Eigen::Transform<float, 3, Eigen::Affine> updateTransform = Eigen::Transform<float, 3, Eigen::Affine>::Identity();
if(mTransformationType == IterativeClosestPoint::RIGID) {
// Create correlation matrix H of the deviations from centroid
MatrixXf H = (movedPoints.colwise() - centroidMoving)*
(rearrangedFixedPoints.colwise() - centroidFixed).transpose();
// Do SVD on H
Eigen::JacobiSVD<Eigen::MatrixXf> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
// Estimate rotation as R=V*U.transpose()
MatrixXf temp = svd.matrixV()*svd.matrixU().transpose();
Matrix3f d = Matrix3f::Identity();
d(2,2) = sign(temp.determinant());
Matrix3f R = svd.matrixV()*d*svd.matrixU().transpose();
// Estimate translation
Vector3f T = centroidFixed - R*centroidMoving;
updateTransform.linear() = R;
updateTransform.translation() = T;
} else {
// Only translation
Vector3f T = centroidFixed - centroidMoving;
updateTransform.translation() = T;
}
// Update current transformation
currentTransformation = updateTransform*currentTransformation;
// Calculate RMS error
MatrixXf distance = rearrangedFixedPoints - currentTransformation*(movingPoints.colwise().homogeneous());
error = 0;
for(uint i = 0; i < distance.cols(); i++) {
error += pow(distance.col(i).norm(),2);
}
error = sqrt(error / distance.cols());
iterations++;
reportInfo() << "Error: " << error << Reporter::end;
// To continue, change in error has to be above min error change and nr of iterations less than max iterations
} while(previousError-error > mMinErrorChange && iterations < mMaxIterations);
if(invertTransform){
currentTransformation = currentTransformation.inverse();
}
mError = error;
mTransformation->matrix() = currentTransformation.matrix();
}
int main(void)
{
cout << "Eigen v" << EIGEN_WORLD_VERSION << "." << EIGEN_MAJOR_VERSION << "." << EIGEN_MINOR_VERSION << endl;
static const int R = 288;
static const int N = R*(R+1)/2;
static const int M = 63;
static const int HALF_M = M/2;
static const float nsigma = 2.5f;
MatrixXf data = MatrixXf::Random(M, N);
MatrixXf mask = MatrixXf::Zero(M, N);
MatrixXf result = MatrixXf::Zero(1, N);
VectorXf std = VectorXf::Zero(N);
VectorXf centroid = VectorXf::Zero(N);
VectorXf mean = VectorXf::Zero(N);
VectorXf minval = VectorXf::Zero(N);
VectorXf maxval = VectorXf::Zero(N);
cout << "computing..." << flush;
double t = GetRealTime();
// computes the exact median
if (M&1)
{
#pragma omp parallel for
for (int i = 0; i < N; i++)
{
vector<float> row(data.data()+i*M, data.data()+(i+1)*M);
nth_element(row.begin(), row.begin()+HALF_M, row.end());
centroid(i) = row[HALF_M];
}
}
// nth_element guarantees x_0,...,x_{n-1} < x_n
else
{
#pragma omp parallel for
for (int i = 0; i < N; i++)
{
vector<float> row(data.data()+i*M, data.data()+(i+1)*M);
nth_element(row.begin(), row.begin()+HALF_M, row.end());
centroid(i) = row[HALF_M];
centroid(i) += *max_element(row.begin(), row.begin()+HALF_M);
centroid(i) *= 0.5f;
}
}
// compute the mean
mean = data.colwise().mean();
// compute std (x) = sqrt ( 1/N SUM_i (x(i) - mean(x))^2 )
std = (((data.rowwise() - mean.transpose()).array().square()).colwise().sum() *
(1.0f / M))
.array()
.sqrt();
// compute n sigmas from centroid
minval = centroid - std * nsigma;
maxval = centroid + std * nsigma;
// compute clip mask
for (int i = 0; i < N; i++)
{
mask.col(i) = (data.col(i).array() > minval(i)).select(VectorXf::Ones(M), 0.0f);
mask.col(i) = (data.col(i).array() < maxval(i)).select(VectorXf::Ones(M), 0.0f);
}
// apply clip mask to data
data.array() *= mask.array();
// compute mean such that we ignore clipped data, this is our final result
result = data.colwise().sum().array() / mask.colwise().sum().array();
t = GetRealTime() - t;
cout << "[done]" << endl << endl;
size_t bytes = data.size()*sizeof(float);
cout << "data: " << M << "x" << N << endl;
cout << "size: " << bytes*1e-6f << " MB" << endl;
cout << "rate: " << bytes/(1e6f*t) << " MB/s" << endl;
cout << "time: " << t << " s" << endl;
return 0;
}
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;
}