void operator()(
const Eigen::MatrixXf &A,
const Eigen::MatrixXf &B,
Eigen::MatrixXf &Transform){
assert(A.rows() == B.rows());
assert(A.cols() == numColumnElements);
assert(B.cols() == numColumnElements);
Transform.resize(numColumnElements, numColumnElements);
/*
Transform.col(0) = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B.col(0));
Transform.col(1) = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B.col(1));
Transform.col(2) = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B.col(2));
*/
Transform = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(B);
}
void
computeHistogram (const Eigen::MatrixXf &data, Eigen::MatrixXf &histogram, size_t bins, float min, float max)
{
float bin_size = (max-min) / bins;
int num_dim = data.cols();
histogram = Eigen::MatrixXf::Zero (bins, num_dim);
for (int dim = 0; dim < num_dim; dim++)
{
omp_lock_t bin_lock[bins];
for(size_t pos=0; pos<bins; pos++)
omp_init_lock(&bin_lock[pos]);
#pragma omp parallel for firstprivate(min, max, bins) schedule(dynamic)
for (int j = 0; j<data.rows(); j++)
{
int pos = std::floor( (data(j,dim) - min) / bin_size);
if(pos < 0)
pos = 0;
if(pos > (int)bins)
pos = bins - 1;
omp_set_lock(&bin_lock[pos]);
histogram(pos,dim)++;
omp_unset_lock(&bin_lock[pos]);
}
for(size_t pos=0; pos<bins; pos++)
omp_destroy_lock(&bin_lock[pos]);
}
}
void coordinateDescentLasso(
const Eigen::MatrixXf &data,
const Eigen::VectorXf &output,
Eigen::VectorXf &weights,
const float lambda,
const int nIters,
const bool verbose)
{
const int nExamples = data.rows();
const int nFeatures = data.cols();
for(int iter = 0; iter < nIters; ++iter){
const int featureInd = iter % nFeatures;
float rho = 0;
for(int i = 0; i < nExamples; ++i)
rho += residualWithoutKWeight(
weights,
data.row(i).transpose(),
output[i],
featureInd) * data(i, featureInd);
auto column = data.col(featureInd);
float sumOfColumn = (column.transpose() * column).sum();
weights[featureInd] = coordinateDescentStepLasso(weights[featureInd], sumOfColumn, rho, lambda);
if(verbose){
const float err = rss(weights, data, output);
std::cout << "iter " << iter << " err " << err << std::endl;
std::cout << weights << std::endl;
}
}
}
template <typename PointInT, typename PointOutT> void
pcl::IntensitySpinEstimation<PointInT, PointOutT>::computeIntensitySpinImage (
const PointCloudIn &cloud, float radius, float sigma,
int k,
const std::vector<int> &indices,
const std::vector<float> &squared_distances,
Eigen::MatrixXf &intensity_spin_image)
{
// Determine the number of bins to use based on the size of intensity_spin_image
int nr_distance_bins = static_cast<int> (intensity_spin_image.cols ());
int nr_intensity_bins = static_cast<int> (intensity_spin_image.rows ());
// Find the min and max intensity values in the given neighborhood
float min_intensity = std::numeric_limits<float>::max ();
float max_intensity = -std::numeric_limits<float>::max ();
for (int idx = 0; idx < k; ++idx)
{
min_intensity = (std::min) (min_intensity, cloud.points[indices[idx]].intensity);
max_intensity = (std::max) (max_intensity, cloud.points[indices[idx]].intensity);
}
float constant = 1.0f / (2.0f * sigma_ * sigma_);
// Compute the intensity spin image
intensity_spin_image.setZero ();
for (int idx = 0; idx < k; ++idx)
{
// Normalize distance and intensity values to: 0.0 <= d,i < nr_distance_bins,nr_intensity_bins
const float eps = std::numeric_limits<float>::epsilon ();
float d = static_cast<float> (nr_distance_bins) * std::sqrt (squared_distances[idx]) / (radius + eps);
float i = static_cast<float> (nr_intensity_bins) *
(cloud.points[indices[idx]].intensity - min_intensity) / (max_intensity - min_intensity + eps);
if (sigma == 0)
{
// If sigma is zero, update the histogram with no smoothing kernel
int d_idx = static_cast<int> (d);
int i_idx = static_cast<int> (i);
intensity_spin_image (i_idx, d_idx) += 1;
}
else
{
// Compute the bin indices that need to be updated (+/- 3 standard deviations)
int d_idx_min = (std::max)(static_cast<int> (floor (d - 3*sigma)), 0);
int d_idx_max = (std::min)(static_cast<int> (ceil (d + 3*sigma)), nr_distance_bins - 1);
int i_idx_min = (std::max)(static_cast<int> (floor (i - 3*sigma)), 0);
int i_idx_max = (std::min)(static_cast<int> (ceil (i + 3*sigma)), nr_intensity_bins - 1);
// Update the appropriate bins of the histogram
for (int i_idx = i_idx_min; i_idx <= i_idx_max; ++i_idx)
{
for (int d_idx = d_idx_min; d_idx <= d_idx_max; ++d_idx)
{
// Compute a "soft" update weight based on the distance between the point and the bin
float w = expf (-powf (d - static_cast<float> (d_idx), 2.0f) * constant - powf (i - static_cast<float> (i_idx), 2.0f) * constant);
intensity_spin_image (i_idx, d_idx) += w;
}
}
}
}
}
int main(int argc, char** argv)
{
std::string p_in = "";
std::string p_out = "out.tsv";
namespace po = boost::program_options;
po::options_description desc;
desc.add_options()
("help", "produce help message")
("density", po::value(&p_in), "filename for input density")
("out", po::value(&p_out), "filename for smoothed output density")
;
po::variables_map vm;
po::store(po::parse_command_line(argc, argv, desc), vm);
po::notify(vm);
if(vm.count("help")) {
std::cerr << desc << std::endl;
return 1;
}
Eigen::MatrixXf rho = density::LoadDensity(p_in);
std::cout << "Loaded density dim=" << rho.rows() << "x" << rho.cols() << ", sum=" << rho.sum() << "." << std::endl;
Eigen::MatrixXf result;
{
boost::timer::auto_cpu_timer t;
result = density::DensityAdaptiveSmooth(rho);
}
density::SaveDensity(p_out, result);
std::cout << "Wrote result to file '" << p_out << "'." << std::endl;
return 1;
}
void addEigenMatrixRow( Eigen::MatrixXf &m )
{
Eigen::MatrixXf temp=m;
m.resize(m.rows()+1,m.cols());
m.setZero();
m.block(0,0,temp.rows(),temp.cols())=temp;
}
int run() {
// store cooccurrence in Eigen sparse matrix object
REDSVD::SMatrixXf A;
const int ncontext = read_cooccurrence(c_cooc_file_name, A, verbose);
// read U matrix from file
Eigen::MatrixXf V;
read_eigen_truncated_matrix(c_input_file_V_name, V, dim);
// read S matrix from file
Eigen::VectorXf S;
read_eigen_vector(c_input_file_S_name, S, dim, 1.0-eig);
// checking the dimensions
if (V.rows() != ncontext){
throw std::runtime_error("size mismatch between projection V matrix and the number of context words!!");
}
// starting projection
if (verbose) fprintf(stderr, "Running the projection...");
const double start = REDSVD::Util::getSec();
Eigen::MatrixXf embeddings = A * V * S.asDiagonal().inverse();
if (norm) embeddings.rowwise().normalize();
if (verbose) fprintf(stderr, "done in %.2f.\n",REDSVD::Util::getSec() - start);
// write out embeddings
const char *c_output_name = get_full_path(c_cooc_dir_name, c_output_file_name);
if (verbose) fprintf(stderr, "writing infered word embeddings in %s\n", c_output_name);
write_eigen_matrix(c_output_name, embeddings);
free((char*)c_output_name);
return 0;
}
// draw all the lines between aPts and Pts that have a corr>threshold.
// if aPtInd is in the range of aPts, then draw only the lines that comes from aPts[aPtInd]
void drawCorrLines(PlotLines::Ptr lines, const vector<btVector3>& aPts, const vector<btVector3>& bPts, const Eigen::MatrixXf& corr, float threshold, int aPtInd) {
vector<btVector3> linePoints;
vector<btVector4> lineColors;
float max_corr = corr.maxCoeff(); // color lines by corr, where corr has been mapped [threshold,max_corr] -> [0,1]
for (int i=0; i<corr.rows(); ++i)
for (int j=0; j<corr.cols(); ++j)
if (corr(i,j) > threshold) {
if (aPtInd<0 || aPtInd>=corr.rows() || aPtInd==i) {
linePoints.push_back(aPts[i]);
linePoints.push_back(bPts[j]);
float color_factor = (corr(i,j)-threshold)/(max_corr-threshold); //basically, it ranges from 0 to 1
lineColors.push_back(btVector4(color_factor, color_factor,0,1));
}
}
lines->setPoints(linePoints, lineColors);
}
// Send a matrix to MATLAB
IGL_INLINE void igl::mlsetmatrix(Engine** mlengine, std::string name, const Eigen::MatrixXf& M)
{
if (*mlengine == 0)
mlinit(mlengine);
mxArray *A = mxCreateDoubleMatrix(M.rows(), M.cols(), mxREAL);
double *pM = mxGetPr(A);
int c = 0;
for(int j=0; j<M.cols();++j)
for(int i=0; i<M.rows();++i)
pM[c++] = double(M(i,j));
engPutVariable(*mlengine, name.c_str(), A);
mxDestroyArray(A);
}
IGL_INLINE void igl::mlgetmatrix(Engine** mlengine, std::string name, Eigen::MatrixXf& M)
{
if (*mlengine == 0)
mlinit(mlengine);
unsigned long m = 0;
unsigned long n = 0;
std::vector<double> t;
mxArray *ary = engGetVariable(*mlengine, name.c_str());
if (ary == NULL)
{
m = 0;
n = 0;
M = Eigen::MatrixXf(0,0);
}
else
{
m = mxGetM(ary);
n = mxGetN(ary);
M = Eigen::MatrixXf(m,n);
double *pM = mxGetPr(ary);
int c = 0;
for(int j=0; j<M.cols();++j)
for(int i=0; i<M.rows();++i)
M(i,j) = pM[c++];
}
mxDestroyArray(ary);
}
Eigen::MatrixXf
readDescrFromFile(const std::string &file, int padding, int rowSize)
{
// check if file exists
boost::filesystem::path path = file;
if ( ! (boost::filesystem::exists ( path ) && boost::filesystem::is_regular_file(path)) )
throw std::runtime_error ("Given file path to read Matrix does not exist!");
std::ifstream in (file.c_str (), std::ifstream::in);
int bufferSize = 819200;
//int bufferSize = rowSize * 10;
char linebuf[bufferSize];
Eigen::MatrixXf matrix;
matrix.resize(0,rowSize);
int j=0;
while(in.getline (linebuf, bufferSize)){
int start_s=clock();
std::string line (linebuf);
std::vector < std::string > strs_2;
boost::split (strs_2, line, boost::is_any_of (" "));
matrix.conservativeResize(matrix.rows()+1,rowSize);
for (int i = 0; i < strs_2.size()-1; i++)
matrix (j, i) = static_cast<float> (atof (strs_2[i].c_str ()));
j++;
int stop_s=clock();
std::cout << "time: " << (stop_s-start_s)/double(CLOCKS_PER_SEC)*1000 << std::endl;
}
return matrix;
}
Eigen::MatrixXf SumMipMap(const Eigen::MatrixXf& img_big)
{
// size of original image
const unsigned int w_big = img_big.rows();
const unsigned int h_big = img_big.cols();
// size of reduced image
const unsigned int w_sma = w_big / Q;
const unsigned int h_sma = h_big / Q;
// the computed mipmap will have 2^i size
if(Q*w_sma != w_big || Q*h_sma != h_big) {
throw std::runtime_error("ERROR: Q and size does not match in function SumMipMap!");
}
Eigen::MatrixXf img_small(w_sma, h_sma);
for(unsigned int y=0; y<h_sma; ++y) {
const unsigned int y_big = Q*y;
for(unsigned int x=0; x<w_sma; ++x) {
const unsigned int x_big = Q*x;
float sum = 0.0f;
for(unsigned int i=0; i<Q; ++i) {
for(unsigned int j=0; j<Q; ++j) {
sum += img_big(x_big+j, y_big+i);
}
}
img_small(x, y) = sum;
}
}
return img_small;
}
std::vector<Eigen::Vector2f> RectGrid(const Eigen::MatrixXf& density)
{
const float width = static_cast<float>(density.rows());
const float height = static_cast<float>(density.cols());
const float numf = density.sum();
const float d = std::sqrt(float(width*height) / numf);
const unsigned int Nx = static_cast<unsigned int>(std::ceil(width / d));
const unsigned int Ny = static_cast<unsigned int>(std::ceil(height / d));
const float Dx = width / static_cast<float>(Nx);
const float Dy = height / static_cast<float>(Ny);
const float Hx = Dx/2.0f;
const float Hy = Dy/2.0f;
std::vector<Eigen::Vector2f> seeds;
seeds.reserve(Nx*Ny);
for(unsigned int iy=0; iy<Ny; iy++) {
float y = Hy + Dy * static_cast<float>(iy);
for(unsigned int ix=0; ix<Nx; ix++) {
float x = Hx + Dx * static_cast<float>(ix);
seeds.push_back(Eigen::Vector2f(x, y));
}
}
return seeds;
}
void sumAndNormalize( Eigen::MatrixXf & out, const Eigen::MatrixXf & in, const Eigen::MatrixXf & Q ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<in.cols(); i++ ){
Eigen::VectorXf b = in.col(i);
Eigen::VectorXf q = Q.col(i);
out.col(i) = b.array().sum()*q - b;
}
}
void Eigen_to_G(const Eigen::MatrixXf& EigenM, Matrix* _GM)
{
Matrix& GM = *_GM;
GM.Dimension(EigenM.rows(), EigenM.cols());
for(int i=0; i<GM.rows; i++)
for(int j=0; j<GM.cols; j++)
GM[i][j] = EigenM(i, j);
}
void eigen2mat(Eigen::MatrixXf &red, Eigen::MatrixXf &green, Eigen::MatrixXf &blue, cv::Mat& dst)
{
if (red.cols() != blue.cols() || red.cols() != green.cols() || blue.cols() != green.cols())
{
std::cerr << "Error: Different number of columns in source channels" << std::endl;
return;
}
if (red.rows() != blue.rows() || red.rows() != green.rows() || blue.rows() != green.rows())
{
std::cerr << "Error: Different number of rows in source channels" << std::endl;
return;
}
//-- Eigen to OpenCV to convert RGB image as image (Quick and dirty)
//------------------------------------------------------------------------------
int width = red.cols();
int height = red.rows();
dst= cv::Mat(height, width, CV_8UC3);
uint8_t* image_ptr = (uint8_t*)dst.data;
for (int i = 0; i < height; i++)
for (int j = 0; j < width; j++)
{
image_ptr[i*dst.cols*3 + j*3 + 0] = blue(i, j); // B
image_ptr[i*dst.cols*3 + j*3 + 1] = green(i, j);// G
image_ptr[i*dst.cols*3 + j*3 + 2] = red(i, j); // R
}
}
IGL_INLINE void igl::fit_rotations_AVX(
const Eigen::MatrixXf & S,
Eigen::MatrixXf & R)
{
const int cStep = 8;
assert(S.cols() == 3);
const int dim = 3; //S.cols();
const int nr = S.rows()/dim;
assert(nr * dim == S.rows());
// resize output
R.resize(dim,dim*nr); // hopefully no op (should be already allocated)
Eigen::Matrix<float, 3*cStep, 3> siBig;
// using SSE decompose cStep matrices at a time:
int r = 0;
for( ; r<nr; r+=cStep)
{
int numMats = cStep;
if (r + cStep >= nr) numMats = nr - r;
// build siBig:
for (int k=0; k<numMats; k++)
{
for(int i = 0;i<dim;i++)
{
for(int j = 0;j<dim;j++)
{
siBig(i + 3*k, j) = S(i*nr + r + k, j);
}
}
}
Eigen::Matrix<float, 3*cStep, 3> ri;
polar_svd3x3_avx(siBig, ri);
for (int k=0; k<cStep; k++)
assert(ri.block(3*k, 0, 3, 3).determinant() >= 0);
// Not sure why polar_dec computes transpose...
for (int k=0; k<numMats; k++)
{
R.block(0, (r + k)*dim, dim, dim) = ri.block(3*k, 0, dim, dim).transpose();
}
}
}
//uses cauchy as cost function instead of squared error
//observations is a matrix of nx1 where n is the number of landmarks observed
//each value in the matrix represents the angle at which the landmark was observed
//params is a matrix of nx2 where n is the number of landmarks
//for each landmark, the x and y pose of where it is
//pose is a matrix of 2x1 containing the initial guess of the robot pose
//error is a nx1 matrix for the difference between the measurement and the estimated angle
double LMAlgr::computeError(Eigen::MatrixXf observations, Eigen::MatrixXf params, Eigen::MatrixXf pose, Eigen::MatrixXf &error){
Eigen::MatrixXf estimated_angle;
estimated_angle.resize(observations.rows(), observations.cols());
for(int i = 0; i < observations.rows(); i++){
//compute the estimated angle for each landmark
estimated_angle(i, 0) = atan2(params(i, 1) - pose(1, 0), params(i, 0) - pose(0, 0));
//std::cout << params(i, 1) << " " << params(i, 0) << " " << pose(1, 0) << " " << pose(0, 0) << " " << estimated_angle(i, 0) << " " << observations(i, 0) << std::endl;
//compute the error for each landmark
error(i, 0) = atan2(sin(observations(i, 0) - estimated_angle(i, 0)), cos(observations(i, 0) - estimated_angle(i, 0)));//normalize_angle(observations(i, 0) - estimated_angle(i, 0));
}
/*std::cout << "estimated angle: \n" << estimated_angle << std::endl;*/
/*std::cout << "error matrix: \n " << error << std::endl;*/
//std::cout << "final error value:\n" << error << std::endl;
double cost = outlier_threshold * outlier_threshold * log10(1 + (error.squaredNorm() / (outlier_threshold * outlier_threshold)));
double weight = sqrt(cost) / error.norm();
error = weight * error;
return error.squaredNorm();
}
///////////////////////
///// Inference /////
///////////////////////
void expAndNormalize ( Eigen::MatrixXf & out, const Eigen::MatrixXf & in ) {
out.resize( in.rows(), in.cols() );
for( int i=0; i<out.cols(); i++ ){
Eigen::VectorXf b = in.col(i);
b.array() -= b.maxCoeff();
b = b.array().exp();
out.col(i) = b / b.array().sum();
}
}
void VertexBufferObject::update(const Eigen::MatrixXf& M)
{
assert(id != 0);
glBindBuffer(GL_ARRAY_BUFFER, id);
glBufferData(GL_ARRAY_BUFFER, sizeof(float)*M.size(), M.data(), GL_DYNAMIC_DRAW);
rows = M.rows();
cols = M.cols();
check_gl_error();
}
bool ZMeshBilateralFilter::apply(const Eigen::MatrixXf& input, int spatialDim, int rangeDim, const Eigen::VectorXf& weights, const std::vector<bool>& tags)
{
setRangeDim(rangeDim);
setSpatialDim(spatialDim);
Eigen::VectorXf weights2;
std::vector<bool> tags2;
bool bW = (weights.size()==0);
bool bT = (tags.empty());
if (bW)
{
weights2.resize(input.rows());
weights2.fill(1.f);
}
if (bT)
{
tags2.resize(input.rows(), true);
}
return apply(input, bW ? weights2 :weights, bT ? tags2 : tags);
}
void getBetaSigma2(double delta) {
Eigen::MatrixXf x = (this->lambda.array() + delta).sqrt().matrix().asDiagonal() * this->ux;
Eigen::MatrixXf y = (this->lambda.array() + delta).sqrt().matrix().asDiagonal() * this->uy;
this->beta = (x.transpose() * x).eval().ldlt().solve(x.transpose() * y);
double sumResidual2 = getSumResidual2(delta);
// if ( model == GrammarGamma::MLE) {
this->sigma2_g = sumResidual2 / x.rows();
// } else {
// this->sigma2 = sumResidual2 / (x.rows() - x.cols());
// }
}
void plotObsBorder(const Eigen::MatrixXf cloud, PointCloudPlot::Ptr plot) {
osg::ref_ptr<osg::Vec3Array> centers = new osg::Vec3Array;
osg::ref_ptr<osg::Vec4Array> colors = new osg::Vec4Array;
for (int i=0; i < cloud.rows(); i++) {
if (cloud(i,6)) {
centers->push_back(osg::Vec3f(cloud(i,0), cloud(i,1), cloud(i,2)));
colors->push_back(osg::Vec4f(cloud(i,5), cloud(i,4), cloud(i,3), 0.5));
}
}
plot->setPoints(centers, colors);
}
virtual Eigen::VectorXf parameters() const {
if (ktype_ == CONST_KERNEL)
return Eigen::VectorXf();
else if (ktype_ == DIAG_KERNEL)
return parameters_;
else {
Eigen::MatrixXf p = parameters_;
p.resize( p.cols()*p.rows(), 1 );
return p;
}
}
void load( Archive & ar,
Eigen::MatrixXf & t,
const unsigned int file_version )
{
int n0;
ar >> BOOST_SERIALIZATION_NVP( n0 );
int n1;
ar >> BOOST_SERIALIZATION_NVP( n1 );
t.resize( n0, n1 );
ar >> make_array( t.data(), t.rows()*t.cols() );
}
virtual Eigen::VectorXf gradient( const Eigen::MatrixXf & a, const Eigen::MatrixXf & b ) const {
if (ktype_ == CONST_KERNEL)
return Eigen::VectorXf();
Eigen::MatrixXf fg = featureGradient( a, b );
if (ktype_ == DIAG_KERNEL)
return (f_.array()*fg.array()).rowwise().sum();
else {
Eigen::MatrixXf p = fg*f_.transpose();
p.resize( p.cols()*p.rows(), 1 );
return p;
}
}
virtual void setParameters( const Eigen::VectorXf & p ) {
if (ktype_ == DIAG_KERNEL) {
parameters_ = p;
initLattice( p.asDiagonal() * f_ );
}
else if (ktype_ == FULL_KERNEL) {
Eigen::MatrixXf tmp = p;
tmp.resize( parameters_.rows(), parameters_.cols() );
parameters_ = tmp;
initLattice( tmp * f_ );
}
}
float Fnorm(Eigen::MatrixXf &f)
{
float norm = 0;
for(int i = 0; i < f.rows(); i++)
{
for(int j = 0; j < f.cols(); j++)
{
norm += f(i, j) * f(i, j);
}
}
return sqrt(norm);
}
void printEigenMatrix(const Eigen::MatrixXf &mat) {
int x = mat.rows ();
int y = mat.cols ();
for (int i=0; i<x; ++i) {
for (int j=0; j<y; ++j) {
printf("% 5.1f ", mat(i,j));
}
printf("\n");
}
printf("\n");
}
int Conversion::convert(const Eigen::MatrixXf & matrix, pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud){
//cloud->empty();
for (int i=0; i<matrix.rows();i++){
pcl::PointXYZ basic_point;
basic_point.x = matrix(i,0);
basic_point.y = matrix(i,1);
basic_point.z = matrix(i,2);
cloud->push_back(basic_point);
}
return 1;
}