VectorXf EMclustering::logsumexp(MatrixXf x, int dim)
{
int r = x.rows();
int c = x.cols();
VectorXf y(r);
MatrixXf tmp1(r,c);
VectorXf tmp2(r);
VectorXf s(r);
y = x.rowwise().maxCoeff();//cerr<<"y"<<y<<endl<<endl;
x = x.colwise() - y;
//cerr<<"x"<<x<<endl<<endl;
tmp1 = x.array().exp();
//cerr<<"t"<<tmp1<<endl<<endl;
tmp2 = tmp1.rowwise().sum();
//cerr<<"t"<<tmp2<<endl<<endl;
s = y.array() + tmp2.array().log();
for(int i=0;i<s.size();i++)
{
if(!isfinite(s(i)))
{
s(i) = y(i);
}
}
y.resize(0);
tmp1.resize(0,0);
tmp2.resize(0);
return s;
}
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;
}
void SortPointsAngle(MatrixXf &x2d){
// the array is formed by [x1 y1 1,
// x2 y2 ]
// compute center point
Vector3f point=x2d.colwise().sum();
point=point/x2d.rows();
vector<float> angles;
int i,j;
// compute angles
for (i=0; i<x2d.rows();i++){
double x=(point(0)-x2d(i,0));
double y=(point(1)-x2d(i,1));
angles.push_back(atan2(y,x)*180 / (3.1416));//degree in radians
}
//////////////////////////////////////////////////////
// now order by angle chossing the minor to mayor
for (i =0; i<x2d.rows();i++){
for(j = i+1; j < x2d.rows(); j ++) {
if(angles[j] < angles[i]) {
float temp_x = x2d(i,0);
float temp_y = x2d(i,1);
x2d(i,0) = x2d(j,0);
x2d(i,1) = x2d(j,1);
x2d(j,0) = temp_x;
x2d(j,1) = temp_y;
}
}
}
}
VectorXf EMclustering::loggausspdf(MatrixXf x, VectorXf mu, MatrixXf sigma)
{
//cerr<<x<<endl<<endl;
//cerr<<mu<<endl<<endl;
//cerr<<sigma<<endl<<endl;
int d = x.rows();
int c = x.cols();
int r_sigma = sigma.rows();
int c_sigma = sigma.cols();
MatrixXf tmpx(x.rows(),x.cols());
tmpx = x.colwise() - mu;
MatrixXf u1(r_sigma,c_sigma);
u1 = sigma.llt().matrixL() ;
MatrixXf u2(u1.cols(),u1.rows());
u2 = u1.adjoint();//cerr<<u2<<endl;
MatrixXf Q(u2.cols(),tmpx.cols());
Q = u1.jacobiSvd(ComputeThinU | ComputeThinV).solve(tmpx);
//cerr<<"q"<<Q<<endl;
VectorXf q(Q.cols());
q = Q.cwiseProduct(Q).colwise().sum();//cerr<<"q"<<q<<endl;
VectorXf tmp1(u2.rows());
tmp1 = u2.diagonal();
tmp1 = tmp1.array().log();
double c1 = tmp1.sum() * 2;
double c2 = d * log(2*PI);//cerr<<c1+c2<<endl;
VectorXf y(q.size());
y = -(c1+c2)/2. - q.array()/2.;
tmpx.resize(0,0);
u1.resize(0,0);
u2.resize(0,0);
Q.resize(0,0);
q.resize(0);
tmp1.resize(0);
return y;
}
void Sphere::calculate_cm_ave_dist (const MatrixXf &rr, VectorXf &cm, float &avep)
{
cm = rr.colwise().mean();
MatrixXf diff = rr.rowwise() - cm.transpose();
avep = diff.rowwise().norm().mean();
}
void MapOptimizer::constrain_direction (MatrixXf & dJ, float tol) const
{
// Enforce the simultaneous constraints
//
// /\x. sum y. dJ(y,x) = 0
// /\y. sum x. dJ(y,x) = 0
//
// We combine the two constraints by iteratively weakly enforcing both:
// Let Px,Py project to the feasible subspaces for constraints 1,2, resp.
// Each projection has eigenvalues in {0,1}.
// We approximate the desired projection Pxy as a linear combination of Px,Py
// Pxy' = 1 - alpha ((1-Px) + (1-Py))
// which has eigenvalues in {1} u [1 - alpha, 1 - 2 alpha].
// Hence Pxy = lim n->infty Pxy'^n, where convergence rate depends on alpha.
// The optimal alpha is 2/3, yielding Pxy' eigenvalues in {1} u [-1/3,1/3],
// and resulting in project_scale = -alpha below.
if (logging) LOG(" constraining direction");
const size_t X = dom.size;
const size_t Y = cod.size;
const MatrixXf & J = m_joint;
const VectorXf & sum_y_J = m_dom_prior;
const VectorXf & sum_x_J = m_cod_prior;
const float sum_xy_J = m_cod_prior.sum();
VectorXf sum_y_dJ(J.cols());
VectorXf sum_x_dJ(J.rows());
// this is iterative, so we hand-optimize by merging loops
const float * restrict J_ = J.data();
const float * restrict sum_y_J_ = sum_y_J.data();
const float * restrict sum_x_J_ = sum_x_J.data();
float * restrict dJ_ = dJ.data();
float * restrict project_y_ = sum_y_dJ.data();
float * restrict project_x_ = sum_x_dJ.data();
const float project_scale = -2/3.0;
Vector<float> accum_x_dJ(Y);
float * restrict accum_x_dJ_ = accum_x_dJ;
// accumulate first projection
accum_x_dJ.zero();
for (size_t x = 0; x < X; ++x) {
const float * restrict dJ_x_ = dJ_ + Y * x;
float accum_y_dJ = 0;
for (size_t y = 0; y < Y; ++y) {
float dJ_xy = dJ_x_[y];
accum_y_dJ += dJ_xy;
accum_x_dJ_[y] += dJ_xy;
}
project_y_[x] = project_scale * accum_y_dJ / sum_y_J_[x];
}
for (size_t y = 0; y < Y; ++y) {
project_x_[y] = project_scale * accum_x_dJ_[y] / sum_x_J_[y];
accum_x_dJ_[y] = 0;
}
// apply previous projection and accumulate next projection
for (size_t iter = 0; iter < 100; ++iter) {
float error = 0;
for (size_t x = 0; x < X; ++x) {
const float * restrict J_x_ = J_ + Y * x;
float * restrict dJ_x_ = dJ_ + Y * x;
float accum_y_dJ = 0;
for (size_t y = 0; y < Y; ++y) {
float dJ_xy = dJ_x_[y] += J_x_[y] * (project_x_[y] + project_y_[x]);
accum_y_dJ += dJ_xy;
accum_x_dJ_[y] += dJ_xy;
}
project_y_[x] = project_scale * accum_y_dJ / sum_y_J_[x];
imax(error, max(-accum_y_dJ, accum_y_dJ));
}
for (size_t y = 0; y < Y; ++y) {
float accum_x_dJ_y = accum_x_dJ_[y];
accum_x_dJ_[y] = 0;
project_x_[y] = project_scale * accum_x_dJ_y / sum_x_J_[y];
imax(error, max(-accum_x_dJ_y, accum_x_dJ_y));
}
if (error < tol) {
if (logging) {
LOG(" after " << (1+iter) << " iterations, error < " << error);
}
break;
}
}
// apply final projection
for (size_t x = 0; x < X; ++x) {
const float * restrict J_x_ = J_ + Y * x;
float * restrict dJ_x_ = dJ_ + Y * x;
for (size_t y = 0; y < Y; ++y) {
dJ_x_[y] += J_x_[y] * (project_x_[y] + project_y_[x]);
}
}
if (debug) {
sum_y_dJ = dJ.colwise().sum();
sum_x_dJ = dJ.rowwise().sum();
float sum_xy_dJ = sum_x_dJ.sum();
DEBUG("max constraint errors = "
<< sqrt(sum_x_dJ.array().square().maxCoeff())<< ", "
<< sqrt(sum_y_dJ.array().square().maxCoeff())<< ", "
<< sum_xy_dJ);
sum_y_dJ.array() /= sum_y_J.array();
sum_x_dJ.array() /= sum_x_J.array();
sum_xy_dJ /= sum_xy_J;
DEBUG("max relative constraints errors = "
<< sqrt(sum_x_dJ.array().square().maxCoeff()) << ", "
<< sqrt(sum_y_dJ.array().square().maxCoeff()) << ", "
<< sum_xy_dJ);
DEBUG("max(|dJ|) = " << dJ.array().abs().maxCoeff()
<< ", rms(dJ) = " << sqrt(dJ.array().square().mean()));
DEBUG("max(J) / min(J) = " << (J.maxCoeff() / J.minCoeff()));
DEBUG("max(sum x. J) / min(sum x. J) = "
<< (sum_x_J.maxCoeff() / sum_x_J.minCoeff()));
DEBUG("max(sum y. J) / min(sum y. J) = "
<< (sum_y_J.maxCoeff() / sum_y_J.minCoeff()));
}
}
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 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();
}
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;
}
void sparsify_soft_relative_to_row_col_max (
const MatrixXf & dense,
MatrixSf & sparse,
float relthresh,
bool ignore_diagonal)
{
ASSERT_LT(0, relthresh);
LOG("sparsifying " << dense.rows() << " x " << dense.cols()
<< " positive matrix to relative threshold " << relthresh);
VectorXf row_max;
VectorXf col_max;
if (ignore_diagonal) {
VectorXf diag = dense.diagonal();
const_cast<MatrixXf &>(dense).diagonal().setZero();
row_max = dense.rowwise().maxCoeff();
col_max = dense.colwise().maxCoeff();
const_cast<MatrixXf &>(dense).diagonal() = diag;
} else {
row_max = dense.rowwise().maxCoeff();
col_max = dense.colwise().maxCoeff();
}
const int I = dense.rows();
const int J = dense.cols();
sparse.resize(I,J);
double sum_dense = 0;
double sum_sparse = 0;
for (int j = 0; j < J; ++j) {
for (int i = 0; i < I; ++i) {
const float dense_ij = dense(i,j);
sum_dense += dense_ij;
const float thresh = relthresh * min(row_max(i), col_max(j));
if (dense_ij > thresh) {
sparse.insert(i,j) = dense_ij;
sum_sparse += dense_ij;
}
}
}
sparse.finalize();
float density = sparse.nonZeros() / float(I * J);
float loss = (sum_dense - sum_sparse) / sum_dense;
LOG("sparsifying to density " << density
<< " loses " << (100 * loss) << "% of mass");
}
MatrixXf EMclustering::kmeans(MatrixXf x, int k)
{
//cerr<<x<<endl;
srand (1);
int n = x.cols();
int d = x.rows();
VectorXi idx(k);
idx(0) = rand()%n;
int count2 =1;
for(int i=1;i<k;i++)
{//cerr<<i<<endl;
int tmpi = rand()%n;
bool flag = true;
while(flag)
{
flag = false;
tmpi = rand()%n;
for(int j=0;j<count2;j++)
{
if(idx(j) == tmpi
|| ifequal(idx(j),tmpi,x))
{
flag = true;
break;
}
}
}
idx(i) = tmpi;
count2++;
}
//cerr<<idx<<endl;
MatrixXf a(d,k);
MatrixXf D(n,k);
VectorXi tmpr(n);
VectorXi tmpcount(k);
for(int i=0;i<k;i++)
{
a.col(i) = x.col(idx(i));
}
/*for(int i=0;i<k;i++)
cerr<<a(0,i)<<" ";
cerr<<endl;*/
int count = 0;
while(count<500)
{
for(int i=0;i<k;i++)
{
D.col(i) = (x.colwise() - a.col(i)).colwise().squaredNorm().transpose();
}
for(int i=0;i<n;i++)
{
D.row(i).minCoeff(&tmpr(i));
}
//cerr<<D<<endl;
a.setZero(d,k);
tmpcount.setZero(k);
for(int i=0;i<n;i++)
{
a.col(tmpr(i)) = (a.col(tmpr(i)).array() + x.col(i).array()).matrix();
tmpcount(tmpr(i))++;
}
//cerr<<a<<endl;
for(int i=0;i<k;i++)
{
a.col(i) = (a.col(i).array()/tmpcount(i)).matrix();
}
//cerr<<a<<endl;
//cerr<<tmpr<<endl;
count ++;
}
MatrixXf r(n,k);
r.setZero(n,k);
for(int i=0;i<n;i++)
{
r(i,tmpr(i)) = 1;
}
cerr<<r<<endl;
idx.resize(0);
a.resize(0,0);
D.resize(0,0);
tmpr.resize(0);
tmpcount.resize(0);
return r;
}
gaussian_model EMclustering::maximization(MatrixXf x, MatrixXf r)
{
int d = x.rows();
int n = x.cols();
int k = r.cols();
//cerr<<x<<endl;
VectorXf nk(r.rows());
nk = r.colwise().sum();
VectorXf w(nk.size());
w = nk/n;
MatrixXf tmp1(x.rows(),r.cols());
tmp1 = x * r;
VectorXf tmp2(nk.size());
tmp2 = nk.array().inverse();
//cerr<<tmp2<<endl<<endl;
MatrixXf mu(x.rows(),r.cols());
mu = tmp1 * tmp2.asDiagonal() ;
MatrixXf *sigma = new MatrixXf[k];
for(int i=0;i<k;i++)
sigma[i].setZero(d,d);
MatrixXf sqrtr(r.rows(),r.cols());
sqrtr = r.cwiseSqrt();
MatrixXf xo(d,n);
MatrixXf tmp3(d,d);
tmp3.setIdentity(d,d);
for(int i=0;i<k;i++)
{
xo = x.colwise() - mu.col(i);
VectorXf tmp4(sqrtr.rows());
tmp4 = sqrtr.col(i);
tmp4 = tmp4.adjoint();
xo = xo* tmp4.asDiagonal();
sigma[i] = xo*xo.adjoint()/nk(i);
sigma[i] = sigma[i] + tmp3*1e-6;
//cerr<<sigma[i]<<endl<<endl;
}
gaussian_model model;
model.mu = mu;
model.sigma = new MatrixXf[k];
for(int i=0;i<k;i++)
model.sigma[i] = sigma[i];
model.weight = w;
nk.resize(0);
w.resize(0);
tmp1.resize(0,0);
tmp2.resize(0);
tmp3.resize(0,0);
mu.resize(0,0);
for(int i=0;i<k;i++)
sigma[i].resize(0,0);
delete [] sigma;
sqrtr.resize(0,0);
xo.resize(0,0);
tmp3.resize(0,0);
//cerr<<"---"<<endl;
model.weight = model.weight.adjoint();
//cerr<<model.weight<<endl<<endl;
//cerr<<model.mu<<endl<<endl;
//for(int i=0;i<k;i++)
//{
// cerr<<model.sigma[i]<<endl<<endl;
//}
return model;
}
