point_operations.h

#pragma once
 
#ifdef min
#undef min
#endif
#ifdef max
#undef max
#endif
 
 
#include "vec.h"
#include "mat.h"
#include "transformations.h"
#include <limits>
#include <vector>
#include <algorithm>
 
 
 
namespace cgv {
namespace math {
 
 
 
template<typename T>
inline vec<T> mean(const mat<T>& points)
{
    unsigned N = points.ncols();
    unsigned M = points.nrows();
    vec<T> mu;
    mu.zeros(M);
    
 
    for(unsigned j = 0; j < N;j++)
    {
        for(unsigned i = 0; i < M; i++)
        {
            mu(i)+=points(i,j);
        }
    }
    mu/=(T)N;
    return mu;
}
 
 
template <typename T>
cgv::math::vec<T> geometric_median(const cgv::math::mat<T>& points, const T& eps=0.0001, const unsigned max_iter=100)
{
    unsigned m = points.cols();
    unsigned n = points.rows();
    
    cgv::math::vec<T> y(n);
    cgv::math::vec<T> ytemp(n);
 
    
    for(unsigned iter = 0; iter < max_iter; iter++)
    {
        ytemp.zeros();
        T W =0;
        for(unsigned j = 0;j < m; j++)
        {
            T invdist = length(points.col(j) - y);
            if(invdist != 0)
                invdist=1.0/invdist;
            
            W += invdist;
            ytemp += invdist*points.col(j);
        }
        ytemp = ytemp/W;
        if(length(y-ytemp) < eps)
            return ytemp;
        else
            y=ytemp;    
    }
    return y;
}
 
template<typename T>
inline vec<T> weighted_mean(const vec<T>& weights,const mat<T>& points)
{
    assert(weights.dim() == points.ncols());
    unsigned N = points.ncols();
    unsigned M = points.nrows();
    vec<T> mu;
    mu.zeros(M);
    
    T sm =0;
    for(unsigned j = 0; j < N;j++)
    {
        for(unsigned i = 0; i < M; i++)
        {
            mu(i)+=weights(j)*points(i,j);
        }
        sm+=weights(j);
    }
    mu/=sm;
    return mu;
}
 
 
 
 
 
template <typename T>
inline mat<T> covmat(const mat<T>& points)
{
    unsigned N = points.ncols();
    unsigned M = points.nrows();
    mat<T> covmat;
    covmat.zeroes(M,M);
    vec<T> m;
    m.zeros(M);
    
    
    for(unsigned c = 0; c < N;c++)
    {
        for(unsigned i = 0; i < M; i++)
        {
            for(unsigned j = 0; j < M; j++)
            {
                covmat(i,j)+=points(i,c)*points(j,c);           
            }
            m(i)+=points(i,c);      
        }
    }
    m/=(T)N;
    covmat-=((T)N)*dyad(m,m);
    covmat/=(T)N;
    
    
    return covmat;
}
 
template <typename T>
inline mat<T> weighted_covmat(const vec<T>& weights,const mat<T>& points)
{
    unsigned N = points.ncols();
    unsigned M = points.nrows();
    mat<T> wcovmat;
    wcovmat.zeros(M,M);
    vec<T> wmean(M);
    wmean.zeros();
    T sumsqrweights=0;
    T sumweights=0;
    
    for(unsigned i = 0; i < N;i++)
    {
        sumweights+=weights(i);
    }
    vec<T> wn=weights/sumweights;
    for(unsigned c = 0; c < N;c++)
    {
        for(unsigned i = 0; i < M;i++)
            wmean(i)+=wn(c)*points.col(i,c);
        sumsqrweights += wn(c)*wn(c);
    }
 
 
 
    for(unsigned c = 0; c < N;c++)
    {
        for(unsigned i = 0; i < M; i++)
        {
            for(unsigned j = 0; j < M; j++)
            {
                wcovmat(i,j)+=wn(c)*(points(i,c)-wmean(i))*(points(j,c)-wmean(j));          
            }
                
        }
    }
    wcovmat/=((T)1.0-sumsqrweights);
    
 
    
    return wcovmat;
}
 
 
template <typename T>
inline void weighted_covmat_and_mean(const vec<T>& weights,const mat<T>& points, mat<T>&wcovmat, vec<T>& wmean)
{
    unsigned N = points.ncols();
    unsigned M = points.nrows();
    wcovmat.zeros(M,M);
    wmean.resize(M);
    wmean.zeros();
    
 
    T sumsqrweights=0;
    T sumweights=0;
    
    for(unsigned i = 0; i < N;i++)
    {
        sumweights+=weights(i);
    }
    vec<T> wn=weights/sumweights;
    for(unsigned c = 0; c < N;c++)
    {
        for(unsigned i = 0; i < M;i++)
            wmean(i)+=wn(c)*points(i,c);
        sumsqrweights += wn(c)*wn(c);
    }
 
 
 
    for(unsigned c = 0; c < N;c++)
    {
        for(unsigned i = 0; i < M; i++)
        {
            for(unsigned j = 0; j < M; j++)
            {
                wcovmat(i,j)+=wn(c)*(points(i,c)-wmean(i))*(points(j,c)-wmean(j));          
            }
                
        }
    }
    wcovmat/=((T)1.0-sumsqrweights);
    
}
 
template <typename T>
inline void covmat_and_mean(const mat<T>& points, mat<T>&covmat, vec<T>& mean)
{
    unsigned N = points.ncols();
    unsigned M = points.nrows();
    covmat.zeros(M,M);
    mean.resize(M);
    mean.zeros();
    
 
    for(unsigned c = 0; c < N;c++)
    {
        for(unsigned i = 0; i < M; i++)
        {
            for(unsigned j = 0; j < M; j++)
            {
                covmat(i,j)+=points(i,c)*points(j,c);           
            }
            mean(i)+=points(i,c);       
        }
    }
    mean/=(T)N;
    covmat-=((T)N)*dyad(mean,mean);
    covmat/=(T)N;
    
}
 
// compute the joint mean of N1+N2 datapoints from mean1 of N1 datapoint and mean2 of N2 datapoints
template <typename T>
inline vec<T> joint_mean(const int N1, const vec<T>& mean1, 
                  const int N2, const vec<T>& mean2)
{
    T a = (T)N1/(T)(N2+N1);
    T b = (T)N2/(T)(N2+N1);
    return a*mean1+b*mean2;
}
 
//compute the joint mean and joint covariance matrix of N1+N2 datapoints 
//from  mean1 and covmat1 of N1 data points and mean2 and covmat2 of N2 data points
template <typename T>
inline void joint_mean_and_covmat(const int N1, const vec<T>& mean1,const mat<T> &covmat1,
                           const int N2, const vec<T>& mean2, const mat<T>&covmat2,
                           int&  jointN, vec<T>& jointmean, mat<T>& jointcovmat)
{
 
    jointN = N2+N1;
    T a = (T)N1/(T)(jointN);
    T b = (T)N2/(T)(jointN);
    jointmean = a*mean1+b*mean2;
 
    jointcovmat = N1*covmat1+N1*dyad(mean1,mean1) + N2*covmat2+N2*dyad(mean2,mean2);
    jointcovmat -= ((T)jointN)*dyad(jointmean,jointmean);
    jointcovmat /= (T)jointN;
    
}
 
 
 
 
 
//finds componentwise minimum vector of all column vectors in the matrix points
template<typename T>
inline vec<T> minimum(const mat<T>& points)
{
    unsigned N = points.ncols();
    unsigned M = points.nrows();
    vec<T> _min(M);
    _min.fill(std::numeric_limits<T>::max());
    
    for(unsigned j = 0; j < N;j++)
    {
        for(unsigned i = 0; i < M; i++)
        {
            _min(i) = _min(i) < points(i,j) ? _min(i) : points(i,j);
        }
    }
    
    return _min;
}
 
//finds componentwise maximum vector of all column vectors in the matrix points
template<typename T>
inline vec<T> maximum(const mat<T>& points)
{
    unsigned N = points.ncols();
    unsigned M = points.nrows();
    vec<T> _max(M);
    _max.fill(1-std::numeric_limits<T>::max());
    
    for(unsigned j = 0; j < N;j++)
    {
        for(unsigned i = 0; i < M; i++)
        {
            _max(i) = _max(i) > points(i,j) ? _max(i) : points(i,j);
        }
    }
    
    return _max;
}
 
 
template <typename T>
inline void swap_XYZ_2_XZY(mat<T>& points)
{
    for(unsigned i = 0; i < points.ncols();i++)
    {
        T v = points(1,i);
        points(1,i) =points(2,i);
        points(2,i) = v;    
    }
}
 
//convert a non-homogeneous vector into a homogeneous vector 
template<typename T>
inline void homog(vec<T>& p)
{
    vec<T> t(p.size()+1);
    for(unsigned i = 0; i < p.size();i++)
    {
        t(i)=p(i);
    }
    t(p.size())=(T)1;
    p=t;
}
 
//convert a homogeneous vector into a non-homogeneous vector 
template<typename T>
inline void unhomog(vec<T>& p)
{
    unsigned N=p.size()-1;
    vec<T> temp(N);
    
    for(unsigned i = 0; i < N;i++)
    {
        temp(i)=p(i)/p(N);
    }
    
    p=temp;
}
 
template<typename T>
inline void homog(mat<T>& points)
{
    mat<T> temp(points.nrows()+1,points.ncols());
    for(unsigned j = 0; j < points.ncols(); j++)
    {
        for(unsigned i = 0; i < points.nrows();i++)
        {
            temp(i,j)=points(i,j);
        }
        temp(points.nrows(),j)=(T)1;
    }
    points=temp;
}
 
template<typename T>
inline void unhomog(mat<T>& points)
{
    unsigned N=points.nrows()-1;
    mat<T> temp(N,points.ncols());
    for(unsigned j = 0; j < points.ncols(); j++)
    {
        for(unsigned i = 0; i < N;i++)
        {
            temp(i,j)=points(i,j)/points(N,j);
        }
    }
    points=temp;
}
 
 
template<typename T>
inline void center_and_scale(mat<T>& points,const T& sf=10)
{
    if(points.ncols() > 0)
    {
        assert(points.nrows() == 3);
        vec<T> minv =minimum(points);
        vec<T> maxv =maximum(points);
        vec<T> t = vec<T>((maxv(0)+minv(0))/2.0,minv(1),(maxv(2)+minv(2))/2.0);
        points = points - dyad(t,ones<T>(points.ncols()));
        T s = max_value(maxv-minv);
        points=scale_33<T>(sf/s,
            sf/s,
            sf/s)*points;
    }
}
 
template<typename T>
inline cgv::math::mat<T> center_and_scale_44(mat<T>& points,const T& sf=1)
{
    if(points.ncols() > 0)
    {
        assert(points.nrows() == 3);
        vec<T> minv =minimum(points);
        vec<T> maxv =maximum(points);
        vec<T> t = vec<T>((maxv(0)+minv(0))/2.0,minv(1),(maxv(2)+minv(2))/2.0);
        T s = max_value(maxv-minv);
        return scale_44<T>(sf/s,
            sf/s,
            sf/s)*translate_44(-t);
    }
    return cgv::math::identity<T>(4);
}
 
 
 
 
}
 
}