inline void polynomial()
{
const size_t np = points.size();
const size_t m = min_of<size_t>(degree+1,np);
//______________________________________________________________
//
// initialize parameters
//______________________________________________________________
mu.make(m);
mu.ldz();
coeff.make(m,0);
typename points_t::iterator pp = points.begin();
//______________________________________________________________
//
// accumulate
//______________________________________________________________
for(size_t i=np;i>0;--i,++pp)
{
const point_t p = *pp;
for(size_t j=m,jm=m-1;j>0;--j,--jm)
{
const T xjm = ipower<T>(p.x,jm);
array<T> &mu_j = mu[j];
coeff[j] += xjm * p.y;
for(size_t k=j,km=jm;k>0;--k,--km)
{
const T xkm = ipower<T>(p.x,km);
mu_j[k] += xkm*xjm;
}
}
}
//______________________________________________________________
//
// symetrisation
//______________________________________________________________
for(size_t j=m;j>0;--j)
{
array<T> &mu_j = mu[j];
for(size_t k=j+1;k<=m;++k)
{
mu_j[k] = mu[k][j];
}
}
//______________________________________________________________
//
// compute parameters
//______________________________________________________________
if(!LU<T>::build(mu))
{
throw libc::exception( EINVAL, "invalid data to smooth" );
}
LU<T>::solve(mu,coeff);
}
std::string print_points(const points_t& vals)
{
std::string ret;
for(unsigned i=0;i<vals.size();i++)
{
char tmp[256];
const npoint& p=vals[i];
sprintf(tmp,"(%d,%d)",p.x,p.y);
ret+=tmp;
if(i+1!=vals.size())ret+=";";
}
return ret;
}
void points2bin(const points_t& pts,data_t& bin)
{
bin.resize(pts.size()*2);
for(unsigned i=0;i<pts.size();i++)
{
const point& p=pts[i];
if(p.x<=-128||p.x>127||p.y<=-128||p.y>127)
throw std::runtime_error("points2bin(): invalid point: ("+
boost::lexical_cast<std::string>(p.x)+","
+boost::lexical_cast<std::string>(p.y)+")");
char x=p.x;
char y=p.y;
bin[i*2]=*reinterpret_cast<const unsigned char*>(&x);
bin[i*2+1]=*reinterpret_cast<const unsigned char*>(&y);
}
}
void bin2points(const data_t& bin,points_t& pts)
{
if((bin.size()%2)!=0)
throw std::runtime_error("bin2points(): (bin.size()%2)!=0");
pts.resize(bin.size()/2);
for(unsigned i=0;i<pts.size();i++)
{
point& p=pts[i];
char x=*reinterpret_cast<const unsigned char*>(&bin[i*2]);
char y=*reinterpret_cast<const unsigned char*>(&bin[i*2+1]);
p.x=x;
p.y=y;
}
}
void field_t::get_empty_around(const point& c,points_t& res,int bound_size) const
{
res.resize(0);
for(int y=c.y-bound_size;y<=c.y+bound_size;y++)
for(int x=c.x-bound_size;x<=c.x+bound_size;x++)
{
point p(x,y);
if(at(p)==st_empty)
res.push_back(p);
}
std::sort(res.begin(),res.end(),less_point_pr());
res.erase(std::unique(res.begin(),res.end()),res.end());
}
void make_unique(points_t& pts)
{
std::sort(pts.begin(),pts.end(),less_point_pr());
pts.erase(
std::unique(pts.begin(),pts.end()),
pts.end());
}
void field_t::get_empty_around(points_t& res,int bound_size) const
{
res.resize(0);
for(unsigned i=0;i<steps.size();i++)
{
const step_t& st=steps[i];
for(int y=st.y-bound_size;y<=st.y+bound_size;y++)
for(int x=st.x-bound_size;x<=st.x+bound_size;x++)
{
point p(x,y);
if(at(p)==st_empty)
res.push_back(p);
}
}
std::sort(res.begin(),res.end(),less_point_pr());
res.erase(std::unique(res.begin(),res.end()),res.end());
}
void
opengv::generateRandom2D3DCorrespondences(
const translation_t & position,
const rotation_t & rotation,
const translations_t & camOffsets,
const rotations_t & camRotations,
size_t numberPoints,
double noise,
double outlierFraction,
bearingVectors_t & bearingVectors,
points_t & points,
std::vector<int> & camCorrespondences,
Eigen::MatrixXd & gt )
{
//initialize point-cloud
double minDepth = 4;
double maxDepth = 8;
for( size_t i = 0; i < (size_t) gt.cols(); i++ )
gt.col(i) = generateRandomPoint( maxDepth, minDepth );
//create the 2D3D-correspondences by looping through the cameras
size_t numberCams = camOffsets.size();
size_t camCorrespondence = 0;
for( size_t i = 0; i < (size_t) gt.cols(); i++ )
{
//get the camera transformation
translation_t camOffset = camOffsets[camCorrespondence];
rotation_t camRotation = camRotations[camCorrespondence];
//store the point
points.push_back(gt.col(i));
//project the point into the viewpoint frame
point_t bodyPoint = rotation.transpose()*(gt.col(i) - position);
//project the point into the camera frame
bearingVectors.push_back(camRotation.transpose()*(bodyPoint - camOffset));
//normalize the bearing-vector to 1
bearingVectors[i] = bearingVectors[i] / bearingVectors[i].norm();
//add noise
if( noise > 0.0 )
bearingVectors[i] = addNoise(noise,bearingVectors[i]);
//push back the camera correspondence
camCorrespondences.push_back(camCorrespondence++);
if(camCorrespondence > (numberCams-1) )
camCorrespondence = 0;
}
//add outliers
//compute the number of outliers based on fraction
size_t numberOutliers = (size_t) floor(outlierFraction*numberPoints);
//make the first numberOutliers points be outliers
for(size_t i = 0; i < numberOutliers; i++)
{
//extract the camera transformation
translation_t camOffset = camOffsets[camCorrespondences[i]];
rotation_t camRotation = camRotations[camCorrespondences[i]];
//create random point
point_t p = generateRandomPoint(8,4);
//project into viewpoint frame
point_t bodyPoint = rotation.transpose()*(p - position);
//project into camera-frame and use as outlier measurement
bearingVectors[i] = camRotation.transpose()*(bodyPoint - camOffset);
//normalize the bearing vector
bearingVectors[i] = bearingVectors[i] / bearingVectors[i].norm();
}
}
//! compute the approximation of Y[i]@X[i]
inline T compute(const expand<T> &xp,
const unit_t i,
const array<T> &X,
const array<T> &Y,
const unit_t N,
T *dYdX=0)
{
assert(N>0);
assert(X.size()==Y.size());
assert(unit_t(X.size())==N);
//______________________________________________________________
//
// prepare with central point
//______________________________________________________________
const T Xc = xp.get_x(i,X,N);
const T Yc = xp.get_y(i,Y,N);
points.free();
push_back(0,Yc);
//______________________________________________________________
//
// add next points
//______________________________________________________________
{
const T Xup = Xc + max_of<T>(0,upper_range);
unit_t iup = i;
size_t cnt = 0;
while(true)
{
const T Xi=xp.get_x(++iup,X,N);
if(Xi>Xup&&cnt>0) break;
++cnt;
push_back(Xi-Xc,xp.get_y(iup,Y,N));
}
}
//______________________________________________________________
//
// add prev points
//______________________________________________________________
{
const T Xdn = Xc - max_of<T>(0,lower_range);
unit_t idn = i;
size_t cnt = 0;
while(true)
{
const T Xi=xp.get_x(--idn,X,N);
if(Xi<Xdn&&cnt>0) break;
++cnt;
push_front(Xi-Xc,xp.get_y(idn,Y,N));
}
}
//______________________________________________________________
//
// use local fit
//______________________________________________________________
polynomial();
if(dYdX)
{
*dYdX = coeff[2];
}
return coeff[1];
}
inline void push_front( T x, T y ) { const point_t p(x,y); points.push_front(p); }
inline void push_back( T x, T y ) { const point_t p(x,y); points.push_back(p); }
inline bool operator> (const points_t &v1, const points_t& v2) {
return (v1.size() > v2.size() );
}
