void sampleHWt(ZZX &poly, long Hwt, long n)
{
if (n<=0) n=deg(poly)+1; if (n<=0) return;
clear(poly); // initialize to zero
poly.SetMaxLength(n); // allocate space for degree-(n-1) polynomial
long b,u,i=0;
if (Hwt>n) Hwt=n;
while (i<Hwt) { // continue until exactly Hwt nonzero coefficients
u=lrand48()%n; // The next coefficient to choose
if (IsZero(coeff(poly,u))) { // if we didn't choose it already
b = lrand48()&2; // b random in {0,2}
b--; // random in {-1,1}
SetCoeff(poly,u,b);
i++; // count another nonzero coefficient
}
}
poly.normalize(); // need to call this after we work on the coeffs
}
ex row_power_z::coeff(const ex &s, int n) const
{
if(s.is_equal(s_taylor))
{
if(n < 0)
return 0;
//DEBUG_OUT(op(0) << "^(" << op(1) << ").Tk.size() = " << Tk.size())
while(n > int(Tk.size()) - 1)
{
const_cast<row_power_z*>(this)->Tk.push_back(const_cast<row_power_z*>(this)->GetSum(Tk.size()));
}
//DEBUG_OUT("Tk[" << n << "] = " << Tk[n])
//DEBUG_OUT("Tk[" << n << "].evalf = " << Tk[n].evalf())
return Tk[n];
}
else if(s.is_equal(s_p))
return coeff(s_taylor, -(n+1));
else
return inherited::coeff(s,n);
}
void LABtoRGB(const double lab[], unsigned char rgb[])
{
MatrixXd coeff(3,3);
coeff << 3.0628971, -1.3931791, -0.4757517,
-0.9692660, 1.8760108, 0.0415560,
0.0678775, -0.2288548, 1.0693490;
Vector3d _lab;
_lab << lab[0], lab[1], lab[2];
Vector3d xyz;
LABtoXYZ(_lab, xyz);
Vector3d _rgb;
_rgb = coeff*xyz;
_rgb *= 255.;
rgb[0] = _rgb[0];
rgb[1] = _rgb[1];
rgb[2] = _rgb[2];
}
// The recursive procedure in the Paterson-Stockmeyer
// polynomial-evaluation algorithm from SIAM J. on Computing, 1973.
// This procedure assumes that poly is monic, deg(poly)=k*(2t-1)+delta
// with t=2^e, and that babyStep contains >= k+delta powers
static void
PatersonStockmeyer(Ctxt& ret, const ZZX& poly, long k, long t, long delta,
DynamicCtxtPowers& babyStep, DynamicCtxtPowers& giantStep)
{
if (deg(poly)<=babyStep.size()) { // Edge condition, use simple eval
simplePolyEval(ret, poly, babyStep);
return;
}
ZZX r = trunc(poly, k*t); // degree <= k*2^e-1
ZZX q = RightShift(poly, k*t); // degree == k(2^e-1) +delta
const ZZ p = to_ZZ(babyStep[0].getPtxtSpace());
const ZZ& coef = coeff(r,deg(q));
SetCoeff(r, deg(q), coef-1); // r' = r - X^{deg(q)}
ZZX c,s;
DivRem(c,s,r,q); // r' = c*q + s
// deg(s)<deg(q), and if c!= 0 then deg(c)<k-delta
assert(deg(s)<deg(q));
assert(IsZero(c) || deg(c)<k-delta);
SetCoeff(s,deg(q)); // s' = s + X^{deg(q)}, deg(s)==deg(q)
// reduce the coefficients modulo p
for (long i=0; i<=deg(c); i++) rem(c[i],c[i], p);
c.normalize();
for (long i=0; i<=deg(s); i++) rem(s[i],s[i], p);
s.normalize();
// Evaluate recursively poly = (c+X^{kt})*q + s'
PatersonStockmeyer(ret, q, k, t/2, delta, babyStep, giantStep);
Ctxt tmp(ret.getPubKey(), ret.getPtxtSpace());
simplePolyEval(tmp, c, babyStep);
tmp += giantStep.getPower(t);
ret.multiplyBy(tmp);
PatersonStockmeyer(tmp, s, k, t/2, delta, babyStep, giantStep);
ret += tmp;
}
void Assembler::operKernel(EOExpr<A> oper,const MeshHandler<ORDER>& mesh,
FiniteElement<Integrator, ORDER>& fe, SpMat& OpMat)
{
std::vector<coeff> triplets;
for(auto t=0; t<mesh.num_triangles(); t++)
{
fe.updateElement(mesh.getTriangle(t));
// Vector of vertices indices (link local to global indexing system)
std::vector<UInt> identifiers;
identifiers.resize(ORDER*3);
for( auto q=0; q<ORDER*3; q++)
identifiers[q]=mesh.getTriangle(t)[q].id();
//localM=localMassMatrix(currentelem);
for(int i = 0; i < 3*ORDER; i++)
{
for(int j = 0; j < 3*ORDER; j++)
{
Real s=0;
for(int l = 0;l < Integrator::NNODES; l++)
{
s += oper(fe,i,j,l) * fe.getDet() * fe.getAreaReference()* Integrator::WEIGHTS[l];//(*)
}
triplets.push_back(coeff(identifiers[i],identifiers[j],s));
}
}
}
UInt nnodes = mesh.num_nodes();
OpMat.resize(nnodes, nnodes);
OpMat.setFromTriplets(triplets.begin(),triplets.end());
//cout<<"done!"<<endl;;
}
bool planeSegmentation(const typename pcl::PointCloud<PointT>::Ptr& inputCloud,
typename pcl::PointCloud<PointT>::Ptr* planeCloud,
typename pcl::PointCloud<PointT>::Ptr* nonPlaneCloud,
pcl::ModelCoefficients::Ptr* coefficients)
{
pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
bool planeFound = false;
if ( coefficients == NULL )
{
pcl::ModelCoefficients::Ptr coeff( new pcl::ModelCoefficients() );
planeFound = locateMainPlane<PointT>(inputCloud, inliers, coeff);
}
else
planeFound = locateMainPlane<PointT>(inputCloud, inliers, *coefficients);
if ( planeFound )
{
if ( planeCloud != NULL )
{
extractIndices<PointT>(inputCloud,
inliers,
false, //points in the plane will survive
*planeCloud);
}
if ( nonPlaneCloud != NULL )
{
extractIndices<PointT>(inputCloud,
inliers,
true,
*nonPlaneCloud);
}
return true;
}
else
return false;
}
int main(int argc, char** argv)
{
if (argc < 2)
{
PCL_ERROR("Usage: %s gt.pcd\n", argv[0]);
exit(1);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudp (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (argv[1], *cloudp) == -1)
{
PCL_ERROR("Failed to load file %s\n", argv[1]);
exit(1);
}
else
PCL_WARN("Loaded PCD %s\n", argv[1]);
pcl::ModelCoefficients::Ptr coeff (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inlier = RansacMethod(cloudp, coeff);
// RaycostMethod(cloudp, center);
printf("%lf %lf %lf %lf\n", coeff->values[0], coeff->values[1], coeff->values[2], coeff->values[3]);
#if 0
// Separate and colorize inliers and outliers
pcl::PointCloud<pcl::PointXYZRGB>::Ptr clr_cloud = ColorizeCloud(cloudp, inlier);
// Display the final result
boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer;
viewer = SetupViewer(clr_cloud, coeff);
while( !viewer->wasStopped())
{
viewer->spinOnce(100);
boost::this_thread::sleep (boost::posix_time::microseconds (100000));
}
#endif
return 0;
}
void Cmod<type>::FFT(zzv &y, const ZZX& x) const
{
FHE_TIMER_START;
zpBak bak; bak.save();
context.restore();
zp rt;
zpx& tmp = getScratch();
conv(tmp,x); // convert input to zpx format
conv(rt, root); // convert root to zp format
BluesteinFFT(tmp, getM(), rt, *powers, powers_aux, *Rb, Rb_aux, *Ra); // call the FFT routine
// copy the result to the output vector y, keeping only the
// entries corresponding to primitive roots of unity
y.SetLength(zMStar->getPhiM());
long i,j;
long m = getM();
for (i=j=0; i<m; i++)
if (zMStar->inZmStar(i)) y[j++] = rep(coeff(tmp,i));
FHE_TIMER_STOP;
}
void Force_fitter::setup(doublevar maxcut, int nexpansion ) {
m=2;
cut=maxcut;
nexp=nexpansion;
coeff.Resize(nexp);
Array2 <doublevar> S(nexp, nexp);
Array1 <doublevar> h(nexp);
for(int i=0; i< nexp; i++) {
for(int j=0; j< nexp; j++) {
S(i,j)=pow(cut,m+i+j+1)/(m+i+j+1);
}
h(i)=pow(cut,i+1)/(i+1);
}
Array2 <doublevar> Sinv(nexp, nexp);
InvertMatrix(S, Sinv, nexp);
coeff=0;
for(int i=0; i< nexp; i++)
for(int j=0;j< nexp;j++)
coeff(i)+=Sinv(i,j)*h(j);
}
/*
* Find the ground in the environment.
* Params[in/out]: the cloud, the coeff of the planes, the indices, the ground cloud, the hull cloud
*/
void Segmentation::removeWall(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud)
{
pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr indices(new pcl::PointIndices);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE); // We search a plane perpendicular to the y
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.023); //0.25
seg.setAxis(Eigen::Vector3f(0,-std::sqrt(2)/2,std::sqrt(2)/2)); // Axis Y 0, -1, 0
seg.setEpsAngle(30.0f * (M_PI/180.0f)); // 50 degrees of error
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
seg.setInputCloud (cloud);
seg.segment (*indices, *coeff);
if (indices->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model (Ground).");
}
else
{
extract.setInputCloud(cloud);
extract.setIndices(indices);
extract.setNegative(true);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGBA>);
extract.filter(*cloud_f);
cloud.swap(cloud_f);
}
}
void LatticeMinimizer::step(const matrix3<>& dir, double alpha)
{ //Project wavefunctions to atomic orbitals:
std::vector<matrix> coeff(e.eInfo.nStates); //best fit coefficients
int nAtomic = e.iInfo.nAtomicOrbitals();
if(e.cntrl.dragWavefunctions && nAtomic)
for(int q=e.eInfo.qStart; q<e.eInfo.qStop; q++)
{ //Get atomic orbitals for old lattice:
e.eVars.Y[q].free();
ColumnBundle psi = e.iInfo.getAtomicOrbitals(q, false);
//Fit the wavefunctions to atomic orbitals (minimize C0^OC0 where C0 is the remainder)
ColumnBundle Opsi = O(psi); //non-trivial cost for uspp
coeff[q] = inv(psi^Opsi) * (Opsi^e.eVars.C[q]);
Opsi.free();
e.eVars.C[q] -= psi * coeff[q]; //now contains residual
}
//Change lattice:
strain += alpha * dir;
e.gInfo.R = Rorig + Rorig*strain; // Updates the lattice vectors to current strain
bcast(e.gInfo.R); //ensure consistency to numerical precision
updateLatticeDependent(true); // Updates lattice information but does not touch electronic state / calc electronic energy
for(int q=e.eInfo.qStart; q<e.eInfo.qStop; q++)
{ //Restore wavefunctions from atomic orbitals:
if(e.cntrl.dragWavefunctions && nAtomic)
{ //Get atomic orbitals for new lattice:
ColumnBundle psi = e.iInfo.getAtomicOrbitals(q, false);
//Reconstitute wavefunctions:
e.eVars.C[q] += psi * coeff[q];
}
//Reorthonormalize wavefunctions:
e.eVars.VdagC[q].clear();
matrix orthoMat = invsqrt(e.eVars.C[q]^O(e.eVars.C[q], &e.eVars.VdagC[q]));
e.eVars.Y[q] = e.eVars.C[q] * orthoMat;
e.eVars.C[q] = e.eVars.Y[q];
e.iInfo.project(e.eVars.C[q], e.eVars.VdagC[q], &orthoMat);
}
}
/**
* Builds and returns the stacked coefficient matrices for both horizontal
* and vertical stacking linOps.
*
* Constructs coefficient matrix COEFF for each argument of LIN. COEFF is
* essentially an identity matrix with an offset to account for stacking.
*
* If the stacking is vertical, the columns of the matrix for each argument
* are interleaved with each other. Otherwise, if the stacking is horizontal,
* the columns are laid out in the order of the arguments.
*
* Parameters: linOP LIN that performs a stacking operation (HSTACK or VSTACK)
* boolean VERTICAL: True if vertical stack. False otherwise.
*
* Returns: vector COEFF_MATS containing the stacked coefficient matrices
* for each argument.
*
*/
std::vector<Matrix> stack_matrices(LinOp &lin, bool vertical) {
std::vector<Matrix> coeffs_mats;
int offset = 0;
int num_args = lin.args.size();
for (int idx = 0; idx < num_args; idx++) {
LinOp arg = *lin.args[idx];
/* If VERTICAL, columns that are interleaved. Otherwise, they are
laid out in order. */
int column_offset;
int offset_increment;
if (vertical) {
column_offset = lin.size[0];
offset_increment = arg.size[0];
} else {
column_offset = arg.size[0];
offset_increment = arg.size[0] * arg.size[1];
}
std::vector<Triplet> tripletList;
tripletList.reserve(arg.size[0] * arg.size[1]);
for (int i = 0; i < arg.size[0]; i++) {
for (int j = 0; j < arg.size[1]; j++) {
int row_idx = i + (j * column_offset) + offset;
int col_idx = i + (j * arg.size[0]);
tripletList.push_back(Triplet(row_idx, col_idx, 1));
}
}
Matrix coeff(lin.size[0] * lin.size[1], arg.size[0] * arg.size[1]);
coeff.setFromTriplets(tripletList.begin(), tripletList.end());
coeff.makeCompressed();
coeffs_mats.push_back(coeff);
offset += offset_increment;
}
return coeffs_mats;
}
static
void compute_a_vals(Vec<ZZ>& a, long p, long e)
// computes a[m] = a(m)/m! for m = p..(e-1)(p-1)+1,
// as defined by Chen and Han.
// a.length() is set to (e-1)(p-1)+2
{
ZZ p_to_e = power_ZZ(p, e);
ZZ p_to_2e = power_ZZ(p, 2*e);
long len = (e-1)*(p-1)+2;
ZZ_pPush push(p_to_2e);
ZZ_pX x_plus_1_to_p = power(ZZ_pX(INIT_MONO, 1) + 1, p);
ZZ_pX denom = InvTrunc(x_plus_1_to_p - ZZ_pX(INIT_MONO, p), len);
ZZ_pX poly = MulTrunc(x_plus_1_to_p, denom, len);
poly *= p;
a.SetLength(len);
ZZ m_fac(1);
for (long m = 2; m < p; m++) {
m_fac = MulMod(m_fac, m, p_to_2e);
}
for (long m = p; m < len; m++) {
m_fac = MulMod(m_fac, m, p_to_2e);
ZZ c = rep(coeff(poly, m));
ZZ d = GCD(m_fac, p_to_2e);
if (d == 0 || d > p_to_e || c % d != 0) Error("cannot divide");
ZZ m_fac_deflated = (m_fac / d) % p_to_e;
ZZ c_deflated = (c / d) % p_to_e;
a[m] = MulMod(c_deflated, InvMod(m_fac_deflated, p_to_e), p_to_e);
}
}
double Constraint::slack(Active<Variable, Constraint> *variables,
double *x) const
{
double eps = master_->machineEps();
double minusEps = -eps;
double c;
double lhs = 0.0;
int n = variables->number();
expand();
for (int i = 0; i < n; i++) {
double xi = x[i];
if (xi > eps || xi < minusEps) {
c = coeff((*variables)[i]);
if (c > eps || c < minusEps)
lhs += c * xi;
}
}
compress();
return rhs() - lhs;
}
// Simple evaluation sum f_i * X^i, assuming that babyStep has enough powers
static void
simplePolyEval(Ctxt& ret, const ZZX& poly, DynamicCtxtPowers& babyStep)
{
ret.clear();
if (deg(poly)<0) return; // the zero polynomial always returns zero
assert (deg(poly)<=babyStep.size()); // ensure that we have enough powers
ZZ coef;
ZZ p = to_ZZ(babyStep[0].getPtxtSpace());
for (long i=1; i<=deg(poly); i++) {
rem(coef, coeff(poly,i),p);
if (coef > p/2) coef -= p;
Ctxt tmp = babyStep.getPower(i); // X^i
tmp.multByConstant(coef); // f_i X^i
ret += tmp;
}
// Add the free term
rem(coef, ConstTerm(poly), p);
if (coef > p/2) coef -= p;
ret.addConstant(coef);
// if (verbose) checkPolyEval(ret, babyStep[0], poly);
}
void RGBtoLABimage(const kn::Image<unsigned char> & src, ImageLab & dest)
{
MatrixXd coeff(3,3);
coeff << 0.4306190, 0.3415419, 0.1783091,
0.2220379, 0.7066384, 0.0713236,
0.0201853, 0.1295504, 0.9390944;
// double xn(0.95045), yn(1.0), zn(1.088754);
for(unsigned j(0); j<src.height(); ++j)
{
for(unsigned i(0); i<src.width(); ++i)
{
Vector3d rgb;
rgb << src(i,j)[0]/255., src(i,j)[1]/255., src(i,j)[2]/255.;
Vector3d xyz;
xyz = coeff*rgb;
dest(i,j)[0] = 116*labFunc(xyz[1]) - 16;
dest(i,j)[1] = 500*( labFunc(xyz[0]) - labFunc(xyz[1]));
dest(i,j)[2] = 200*( labFunc(xyz[1]) - labFunc(xyz[2]));
}
}
}
void
eval_nroots(void)
{
volatile int h, i, k, n;
push(cadr(p1));
eval();
push(caddr(p1));
eval();
p2 = pop();
if (p2 == symbol(NIL))
guess();
else
push(p2);
p2 = pop();
p1 = pop();
if (!ispoly(p1, p2))
stop("nroots: polynomial?");
// mark the stack
h = tos;
// get the coefficients
push(p1);
push(p2);
n = coeff();
if (n > YMAX)
stop("nroots: degree?");
// convert the coefficients to real and imaginary doubles
for (i = 0; i < n; i++) {
push(stack[h + i]);
real();
yyfloat();
eval();
p1 = pop();
push(stack[h + i]);
imag();
yyfloat();
eval();
p2 = pop();
if (!isdouble(p1) || !isdouble(p2))
stop("nroots: coefficients?");
c[i].r = p1->u.d;
c[i].i = p2->u.d;
}
// pop the coefficients
tos = h;
// n is the number of coefficients, n = deg(p) + 1
monic(n);
for (k = n; k > 1; k--) {
findroot(k);
if (fabs(a.r) < DELTA)
a.r = 0.0;
if (fabs(a.i) < DELTA)
a.i = 0.0;
push_double(a.r);
push_double(a.i);
push(imaginaryunit);
multiply();
add();
divpoly(k);
}
// now make n equal to the number of roots
n = tos - h;
if (n > 1) {
sort_stack(n);
p1 = alloc_tensor(n);
p1->u.tensor->ndim = 1;
p1->u.tensor->dim[0] = n;
for (i = 0; i < n; i++)
p1->u.tensor->elem[i] = stack[h + i];
tos = h;
push(p1);
}
}
TEST_F(TestPowerExpansionWithHeavisideCutoff, Gradients) {
typedef std::tuple<int, int, int, double, double, double, bool> nklxyzt_t;
std::vector<nklxyzt_t> nklxyzt_list = {
nklxyzt_t{0,0,0,1.,0.5,-0.5,true},
nklxyzt_t{0,1,2,1.,0.5,-0.5,false},
nklxyzt_t{0,3,1,-1.,-0.5,1.,true},
nklxyzt_t{3,4,3,0.,0.3,-0.7,false},
nklxyzt_t{7,2,4,-0.5,3.,4.2,true}
};
// Second set of positions if same_types = false
double x_2 = 1.5;
double y_2 = -0.5;
double z_2 = 1.;
soap::vec R_2(x_2, y_2, z_2);
double r_2 = soap::linalg::abs(R_2);
soap::vec d_2 = (r_2 > 0.) ? R_2/r_2 : soap::vec(0.,0.,1.);
double weight0_2 = 1.;
double weight_scale_2 = _basis->getCutoff()->calculateWeight(r_2);
std::stringstream output;
std::string output_ref = "n=0 k=0 l=0 x=+1.0000 y=+0.5000 z=-0.5000 X_re=+7.9659943e-02 dX_re=-3.1906261e-01 -1.5953130e-01 +1.5953130e-01 X_im=+0.0000000e+00 dX_im=+0.0000000e+00 +0.0000000e+00 +0.0000000e+00 n=0 k=1 l=2 x=+1.0000 y=+0.5000 z=-0.5000 x_2=+1.0000 y_2=+0.5000 z_2=-0.5000 X_re=+5.3867510e-02 dX_re=-6.0228017e-02 -3.0114009e-02 +3.0114009e-02 X_im=+0.0000000e+00 dX_im=+5.1701290e-18 +0.0000000e+00 -3.4467527e-18 n=0 k=3 l=1 x=-1.0000 y=-0.5000 z=+1.0000 X_re=+2.1246860e-02 dX_re=+2.0772436e-03 +1.0386218e-03 -2.0772436e-03 X_im=+0.0000000e+00 dX_im=+0.0000000e+00 +0.0000000e+00 +0.0000000e+00 n=3 k=4 l=3 x=+0.0000 y=+0.3000 z=-0.7000 x_2=+0.0000 y_2=+0.3000 z_2=-0.7000 X_re=+4.9622475e-06 dX_re=-1.3359712e-04 -1.1974146e-05 +5.6734884e-06 X_im=+4.4449428e-23 dX_im=-7.1119085e-21 +6.2229199e-21 +9.7788741e-21 n=7 k=2 l=4 x=-0.5000 y=+3.0000 z=+4.2000 X_re=+1.0393232e-11 dX_re=+1.3777689e-11 -8.2666133e-11 -1.1573259e-10 X_im=+0.0000000e+00 dX_im=-6.5423244e-29 +2.1028900e-28 +1.4953884e-28 ";
for (auto it = nklxyzt_list.begin(); it != nklxyzt_list.end(); ++it) {
// Extract parameters
int n = std::get<0>(*it);
int k = std::get<1>(*it);
int l = std::get<2>(*it);
double x = std::get<3>(*it);
double y = std::get<4>(*it);
double z = std::get<5>(*it);
bool test_same_types = std::get<6>(*it);
int nk = _basis->getRadBasis()->N()*n+k;
// Distance, direction, weight
soap::vec R(x, y, z);
double r = soap::linalg::abs(R);
soap::vec d = (r > 0.) ? R/r : soap::vec(0.,0.,1.);
double weight0 = 1.;
double weight_scale = _basis->getCutoff()->calculateWeight(r);
double sigma = 0.5;
// Adjust if Xnkl contracted from related densities qnlm (=> same_types = true)
if (test_same_types) {
R_2 = R;
r_2 = r;
d_2 = d;
weight0_2 = weight0;
weight_scale_2 = weight_scale;
}
// Compute
::testing::internal::CaptureStdout();
soap::BasisExpansion basex1(_basis);
basex1.computeCoefficients(r, d, weight0, weight_scale, sigma, true); // <- gradients = true
soap::BasisExpansion basex2(_basis);
basex2.computeCoefficients(r_2, d_2, weight0_2, weight_scale_2, sigma, false); // <- gradients = false
soap::PowerExpansion powex(_basis);
powex.computeCoefficients(&basex1, &basex2); // <- note argument duplication (gradients = false in both cases)
soap::PowerExpansion powex_grad(_basis);
powex_grad.computeCoefficientsGradients(&basex1, &basex2, test_same_types);
::testing::internal::GetCapturedStdout();
// Extract
soap::PowerExpansion::coeff_t &coeff = powex.getCoefficients();
soap::PowerExpansion::coeff_t &coeff_grad_x = powex_grad.getCoefficientsGradX();
soap::PowerExpansion::coeff_t &coeff_grad_y = powex_grad.getCoefficientsGradY();
soap::PowerExpansion::coeff_t &coeff_grad_z = powex_grad.getCoefficientsGradZ();
output
<< boost::format("n=%1$d k=%2$d l=%3$d ") % n % k % l
<< boost::format("x=%1$+1.4f y=%2$+1.4f z=%3$+1.4f ") % x % y % z
<< std::flush;
if (!test_same_types) output
<< boost::format("x_2=%1$+1.4f y_2=%2$+1.4f z_2=%3$+1.4f ") % x % y % z
<< std::flush;
output
<< boost::format("X_re=%1$+1.7e dX_re=%2$+1.7e %3$+1.7e %4$+1.7e ")
% coeff(nk,l).real() % coeff_grad_x(nk,l).real() % coeff_grad_y(nk,l).real() % coeff_grad_z(nk,l).real()
<< boost::format("X_im=%1$+1.7e dX_im=%2$+1.7e %3$+1.7e %4$+1.7e ")
% coeff(nk,l).imag() % coeff_grad_x(nk,l).imag() % coeff_grad_y(nk,l).imag() % coeff_grad_z(nk,l).imag()
<< std::flush;
}
// VERIFY
EXPECT_EQ(output.str(), output_ref);
/*
// MANUAL TESTING
int N = 150;
double dx = 0.05;
bool test_same_types = true;
soap::vec R_2(1.5, -0.5, 1.);
double r_2 = soap::linalg::abs(R_2);
soap::vec d_2 = (r_2 > 0.) ? R_2/r_2 : soap::vec(0.,0.,1.);
double weight0_2 = 1.;
double weight_scale_2 = _basis->getCutoff()->calculateWeight(r_2);
std::string logfile = "tmp";
std::ofstream ofs;
ofs.open(logfile.c_str(), std::ofstream::out);
if (!ofs.is_open()) {
throw std::runtime_error("Bad file handle: " + logfile);
}
for (int i = 0; i < N; ++i) {
double x = i*dx;
double y = 0.5;
double z = -0.2;
// Distance, direction, weight
soap::vec R(x, y, z);
double r = soap::linalg::abs(R);
soap::vec d = (r > 0.) ? R/r : soap::vec(0.,0.,1.);
double weight0 = 1.;
double weight_scale = _basis->getCutoff()->calculateWeight(r);
double sigma = 0.5;
// Adjust if Xnkl contracted from related densities qnlm (=> same_types = true)
if (test_same_types) {
R_2 = R;
r_2 = r;
d_2 = d;
weight0_2 = weight0;
weight_scale_2 = weight_scale;
}
// Compute
::testing::internal::CaptureStdout();
soap::BasisExpansion basex1(_basis);
basex1.computeCoefficients(r, d, weight0, weight_scale, sigma, true); // <- gradients = true
soap::BasisExpansion basex2(_basis);
basex2.computeCoefficients(r_2, d_2, weight0_2, weight_scale_2, sigma, false); // <- gradients = false
soap::PowerExpansion powex(_basis);
powex.computeCoefficients(&basex1, &basex2); // <- note argument duplication (gradients = false in both cases)
soap::PowerExpansion powex_grad(_basis);
powex_grad.computeCoefficientsGradients(&basex1, &basex2, test_same_types);
::testing::internal::GetCapturedStdout();
// Extract
soap::PowerExpansion::coeff_t &coeff = powex.getCoefficients();
soap::PowerExpansion::coeff_t &coeff_grad_x = powex_grad.getCoefficientsGradX();
soap::PowerExpansion::coeff_t &coeff_grad_y = powex_grad.getCoefficientsGradY();
soap::PowerExpansion::coeff_t &coeff_grad_z = powex_grad.getCoefficientsGradZ();
int n = 7;
int k = 5;
int l = 1;
int nk = _basis->getRadBasis()->N()*n+k;
// x (#1) Qnlm_re (#2) dQnlm_dx_re (#4)
ofs << boost::format("%1$+1.4f %2$+1.7e %3$+1.7e %4$+1.7e %5$+1.7e %6$+1.7e %7$+1.7e %8$+1.7e %9$+1.7e")
% R.getX() % coeff(nk,l).real() % coeff(nk,l).imag()
% coeff_grad_x(nk,l).real() % coeff_grad_x(nk,l).imag()
% coeff_grad_y(nk,l).real() % coeff_grad_y(nk,l).imag()
% coeff_grad_z(nk,l).real() % coeff_grad_z(nk,l).imag()
<< std::endl;
}
ofs.close();
*/
}
ZZ sumOfCoeffs(const ZZX& f) // = f(1)
{
ZZ sum = ZZ::zero();
for (long i=0; i<=deg(f); i++) sum += coeff(f,i);
return sum;
}
void KeySwitch::verify(FHESecKey& sk)
{
long fromSPower = fromKey.getPowerOfS();
long fromXPower = fromKey.getPowerOfX();
long fromIdx = fromKey.getSecretKeyID();
long toIdx = toKeyID;
long p = ptxtSpace;
long n = b.size();
cout << "KeySwitch::verify\n";
cout << "fromS = " << fromSPower
<< " fromX = " << fromXPower
<< " fromIdx = " << fromIdx
<< " toIdx = " << toIdx
<< " p = " << p
<< " n = " << n
<< "\n";
if (fromSPower != 1 || fromXPower != 1 || (fromIdx == toIdx) || n == 0) {
cout << "KeySwitch::verify: these parameters not checkable\n";
return;
}
const FHEcontext& context = b[0].getContext();
// we don't store the context in the ks matrix, so let's
// check that they are consistent
for (long i = 0; i < n; i++) {
if (&context != &(b[i].getContext()))
cout << "KeySwitch::verify: bad context " << i << "\n";
}
cout << "context.ctxtPrimes = " << context.ctxtPrimes << "\n";
cout << "context.specialPrimes = " << context.specialPrimes << "\n";
IndexSet allPrimes = context.ctxtPrimes | context.specialPrimes;
cout << "digits: ";
for (long i = 0; i < n; i++)
cout << context.digits[i] << " ";
cout << "\n";
cout << "IndexSets of b: ";
for (long i = 0; i < n; i++)
cout << b[i].getMap().getIndexSet() << " ";
cout << "\n";
// VJS: suspicious shadowing of fromKey, toKey
const DoubleCRT& _fromKey = sk.sKeys.at(fromIdx);
const DoubleCRT& _toKey = sk.sKeys.at(toIdx);
cout << "IndexSet of fromKey: " << _fromKey.getMap().getIndexSet() << "\n";
cout << "IndexSet of toKey: " << _toKey.getMap().getIndexSet() << "\n";
vector<DoubleCRT> a;
a.resize(n, DoubleCRT(context, allPrimes)); // defined modulo all primes
{ RandomState state;
SetSeed(prgSeed);
for (long i = 0; i < n; i++)
a[i].randomize();
} // the RandomState destructor "restores the state" (see NumbTh.h)
vector<ZZX> A, B;
A.resize(n);
B.resize(n);
for (long i = 0; i < n; i++) {
a[i].toPoly(A[i]);
b[i].toPoly(B[i]);
}
ZZX FromKey, ToKey;
_fromKey.toPoly(FromKey, allPrimes);
_toKey.toPoly(ToKey, allPrimes);
ZZ Q = context.productOfPrimes(allPrimes);
ZZ prod = context.productOfPrimes(context.specialPrimes);
ZZX C, D;
ZZX PhimX = context.zMStar.getPhimX();
long nb = 0;
for (long i = 0; i < n; i++) {
C = (B[i] - FromKey*prod + ToKey*A[i]) % PhimX;
PolyRed(C, Q);
if (!divide(D, C, p)) {
cout << "*** not divisible by p at " << i << "\n";
}
else {
for (long j = 0; j <= deg(D); j++)
if (NumBits(coeff(D, j)) > nb) nb = NumBits(coeff(D, j));
}
prod *= context.productOfPrimes(context.digits[i]);
}
cout << "error ratio: " << ((double) nb)/((double) NumBits(Q)) << "\n";
}
/* Copies ith coefficient of x to output.
Assumes output has been mpz_init'd.
AUTHOR: David Harvey (2007-02) */
static CYTHON_INLINE void ZZX_getitem_as_mpz(mpz_t output, struct ZZX* x, long i)
{
const ZZ& z = coeff(*x, i);
ZZ_to_mpz(output, &z);
}
/* Returns ith coefficient of x.
Return value is only valid if the result should fit into an int.
AUTHOR: David Harvey (2006-06-08) */
static CYTHON_INLINE int ZZX_getitem_as_int(struct ZZX* x, long i)
{
return ZZ_to_int(&coeff(*x, i));
}
bool GrGLShaderBuilder::genProgram(const GrEffectStage* colorStages[],
const GrEffectStage* coverageStages[]) {
const GrGLProgramDesc::KeyHeader& header = this->desc().getHeader();
///////////////////////////////////////////////////////////////////////////
// emit code to read the dst copy texture, if necessary
if (kNoDstRead_DstReadKey != header.fDstReadKey && !fGpu->glCaps().fbFetchSupport()) {
bool topDown = SkToBool(kTopLeftOrigin_DstReadKeyBit & header.fDstReadKey);
const char* dstCopyTopLeftName;
const char* dstCopyCoordScaleName;
const char* dstCopySamplerName;
uint32_t configMask;
if (SkToBool(kUseAlphaConfig_DstReadKeyBit & header.fDstReadKey)) {
configMask = kA_GrColorComponentFlag;
} else {
configMask = kRGBA_GrColorComponentFlags;
}
fUniformHandles.fDstCopySamplerUni =
this->addUniform(kFragment_Visibility, kSampler2D_GrSLType, "DstCopySampler",
&dstCopySamplerName);
fUniformHandles.fDstCopyTopLeftUni =
this->addUniform(kFragment_Visibility, kVec2f_GrSLType, "DstCopyUpperLeft",
&dstCopyTopLeftName);
fUniformHandles.fDstCopyScaleUni =
this->addUniform(kFragment_Visibility, kVec2f_GrSLType, "DstCopyCoordScale",
&dstCopyCoordScaleName);
const char* fragPos = this->fragmentPosition();
this->fsCodeAppend("\t// Read color from copy of the destination.\n");
this->fsCodeAppendf("\tvec2 _dstTexCoord = (%s.xy - %s) * %s;\n",
fragPos, dstCopyTopLeftName, dstCopyCoordScaleName);
if (!topDown) {
this->fsCodeAppend("\t_dstTexCoord.y = 1.0 - _dstTexCoord.y;\n");
}
this->fsCodeAppendf("\tvec4 %s = ", kDstCopyColorName);
append_texture_lookup(&fFSCode,
fGpu,
dstCopySamplerName,
"_dstTexCoord",
configMask,
"rgba");
this->fsCodeAppend(";\n\n");
}
///////////////////////////////////////////////////////////////////////////
// get the initial color and coverage to feed into the first effect in each effect chain
GrGLSLExpr4 inputColor;
GrGLSLExpr4 inputCoverage;
if (GrGLProgramDesc::kUniform_ColorInput == header.fColorInput) {
const char* name;
fUniformHandles.fColorUni =
this->addUniform(GrGLShaderBuilder::kFragment_Visibility, kVec4f_GrSLType, "Color",
&name);
inputColor = GrGLSLExpr4(name);
}
if (GrGLProgramDesc::kUniform_ColorInput == header.fCoverageInput) {
const char* name;
fUniformHandles.fCoverageUni =
this->addUniform(GrGLShaderBuilder::kFragment_Visibility, kVec4f_GrSLType, "Coverage",
&name);
inputCoverage = GrGLSLExpr4(name);
} else if (GrGLProgramDesc::kSolidWhite_ColorInput == header.fCoverageInput) {
inputCoverage = GrGLSLExpr4(1);
}
if (k110_GrGLSLGeneration != fGpu->glslGeneration()) {
fFSOutputs.push_back().set(kVec4f_GrSLType,
GrGLShaderVar::kOut_TypeModifier,
declared_color_output_name());
fHasCustomColorOutput = true;
}
this->emitCodeBeforeEffects(&inputColor, &inputCoverage);
///////////////////////////////////////////////////////////////////////////
// emit the per-effect code for both color and coverage effects
GrGLProgramDesc::EffectKeyProvider colorKeyProvider(
&this->desc(), GrGLProgramDesc::EffectKeyProvider::kColor_EffectType);
fColorEffects.reset(this->createAndEmitEffects(colorStages,
this->desc().numColorEffects(),
colorKeyProvider,
&inputColor));
GrGLProgramDesc::EffectKeyProvider coverageKeyProvider(
&this->desc(), GrGLProgramDesc::EffectKeyProvider::kCoverage_EffectType);
fCoverageEffects.reset(this->createAndEmitEffects(coverageStages,
this->desc().numCoverageEffects(),
coverageKeyProvider,
&inputCoverage));
this->emitCodeAfterEffects();
///////////////////////////////////////////////////////////////////////////
// write the secondary color output if necessary
if (GrGLProgramDesc::CoverageOutputUsesSecondaryOutput(header.fCoverageOutput)) {
const char* secondaryOutputName = this->enableSecondaryOutput();
// default coeff to ones for kCoverage_DualSrcOutput
GrGLSLExpr4 coeff(1);
if (GrGLProgramDesc::kSecondaryCoverageISA_CoverageOutput == header.fCoverageOutput) {
// Get (1-A) into coeff
coeff = GrGLSLExpr4::VectorCast(GrGLSLExpr1(1) - inputColor.a());
} else if (GrGLProgramDesc::kSecondaryCoverageISC_CoverageOutput ==
header.fCoverageOutput){
// Get (1-RGBA) into coeff
coeff = GrGLSLExpr4(1) - inputColor;
}
// Get coeff * coverage into modulate and then write that to the dual source output.
this->fsCodeAppendf("\t%s = %s;\n", secondaryOutputName, (coeff * inputCoverage).c_str());
}
///////////////////////////////////////////////////////////////////////////
// combine color and coverage as frag color
// Get "color * coverage" into fragColor
GrGLSLExpr4 fragColor = inputColor * inputCoverage;
// Now tack on "+(1-coverage)dst onto the frag color if we were asked to do so.
if (GrGLProgramDesc::kCombineWithDst_CoverageOutput == header.fCoverageOutput) {
GrGLSLExpr4 dstCoeff = GrGLSLExpr4(1) - inputCoverage;
GrGLSLExpr4 dstContribution = dstCoeff * GrGLSLExpr4(this->dstColor());
fragColor = fragColor + dstContribution;
}
this->fsCodeAppendf("\t%s = %s;\n", this->getColorOutputName(), fragColor.c_str());
if (!this->finish()) {
return false;
}
return true;
}
bitstring Extractor::extract(bitstring w, bitstring x) {
// cout <<"Extractor :: w.size = "<< w.size() <<" n size"<< n<<endl;
// cout <<"Extractor :: x.size = "<< x.size() <<" d size"<< d<<endl;
/*if (w.size() < (size_t)n) {
throw invalid_argument("W has wrong size");
}*/
/*if (x.size() != (size_t)d) {
//cout << "(extractor.cpp:215) TO_REMOVE:X has wrong size (Extractor) x="<< x.size() << "d="<< (size_t)d <<"\n" ;
throw invalid_argument("X has wrong size (Extractor) ");
}*/
// X to poly
ZZ_pE xpoly;
PolyFromBstr(xpoly,x,0);
// W
ZZ_pEX wpoly;
ZZ_pE wpp[32];
for(int i=0; i<32; i++) {
PolyFromBstr(wpp[i],w,32*i);
ZZ_pEX temp(i,wpp[i]);
wpoly += temp;
}
// geracao dos 4 f
ZZ_pEX lixo;
ZZ_pEX f1, f2, f4, f8;
f1 = wpoly;
DivRem(lixo, f1, f1, mod3232);
f2 = f1 * f1;
DivRem(lixo, f2, f2, mod3232);
f4 = f2 * f2;
DivRem(lixo, f4, f4, mod3232);
f8 = f4 * f4;
DivRem(lixo, f8, f8, mod3232);
// f(y)
ZZ_pE res0,res1,res2,res3;
res0 = coeff(f1,0);
res1 = coeff(f2,0);
res2 = coeff(f4,0);
res3 = coeff(f8,0);
ZZ_pE aux = xpoly;
for(int i=1; i<32; i++) {
res0 += aux * coeff(f1,i);
res1 += aux * coeff(f2,i);
res2 += aux * coeff(f4,i);
res3 += aux * coeff(f8,i);
aux *= xpoly;
}
//cout << "RES: " << res0 << "(res0) " << res1 << "(res1) " << res2 << "(res2) " << res2 << "(res2) " << "\n";
bitstring result(128);
ZZ_p one;
one=1;
for(int i = 0; i<32; i++) {
if(coeff(rep(res0),i) == one) result.set(i);
if(coeff(rep(res1),i) == one) result.set(i+32);
if(coeff(rep(res2),i) == one) result.set(i+64);
if(coeff(rep(res3),i) == one) result.set(i+96);
}
/*cout << "Result: ";
result.print();
cout << "\n";*/
// retornar algo de tamanho 32, porque senao da erro de tamanho errado na entrada do extractor.. provavelmente devido a realimentacao
//bitstring bla = x;
//cout <<"bitstring output result = "<< result.to_string()<< endl;
return result;
}
void pose_estimation_from_line_correspondence(Eigen::MatrixXf start_points,
Eigen::MatrixXf end_points,
Eigen::MatrixXf directions,
Eigen::MatrixXf points,
Eigen::MatrixXf &rot_cw,
Eigen::VectorXf &pos_cw)
{
int n = start_points.cols();
if(n != directions.cols())
{
return;
}
if(n<4)
{
return;
}
float condition_err_threshold = 1e-3;
Eigen::VectorXf cosAngleThreshold(3);
cosAngleThreshold << 1.1, 0.9659, 0.8660;
Eigen::MatrixXf optimumrot_cw(3,3);
Eigen::VectorXf optimumpos_cw(3);
std::vector<float> lineLenVec(n,1);
vfloat3 l1;
vfloat3 l2;
vfloat3 nc1;
vfloat3 Vw1;
vfloat3 Xm;
vfloat3 Ym;
vfloat3 Zm;
Eigen::MatrixXf Rot(3,3);
std::vector<vfloat3> nc_bar(n,vfloat3(0,0,0));
std::vector<vfloat3> Vw_bar(n,vfloat3(0,0,0));
std::vector<vfloat3> n_c(n,vfloat3(0,0,0));
Eigen::MatrixXf Rx(3,3);
int line_id;
for(int HowToChooseFixedTwoLines = 1 ; HowToChooseFixedTwoLines <=3 ; HowToChooseFixedTwoLines++)
{
if(HowToChooseFixedTwoLines==1)
{
#pragma omp parallel for
for(int i = 0; i < n ; i++ )
{
// to correct
float lineLen = 10;
lineLenVec[i] = lineLen;
}
std::vector<float>::iterator result;
result = std::max_element(lineLenVec.begin(), lineLenVec.end());
line_id = std::distance(lineLenVec.begin(), result);
vfloat3 temp;
temp = start_points.col(0);
start_points.col(0) = start_points.col(line_id);
start_points.col(line_id) = temp;
temp = end_points.col(0);
end_points.col(0) = end_points.col(line_id);
end_points.col(line_id) = temp;
temp = directions.col(line_id);
directions.col(0) = directions.col(line_id);
directions.col(line_id) = temp;
temp = points.col(0);
points.col(0) = points.col(line_id);
points.col(line_id) = temp;
lineLenVec[line_id] = lineLenVec[1];
lineLenVec[1] = 0;
l1 = start_points.col(0) - end_points.col(0);
l1 = l1/l1.norm();
}
for(int i = 1; i < n; i++)
{
std::vector<float>::iterator result;
result = std::max_element(lineLenVec.begin(), lineLenVec.end());
line_id = std::distance(lineLenVec.begin(), result);
l2 = start_points.col(line_id) - end_points.col(line_id);
l2 = l2/l2.norm();
lineLenVec[line_id] = 0;
MatrixXf cosAngle(3,3);
cosAngle = (l1.transpose()*l2).cwiseAbs();
if(cosAngle.maxCoeff() < cosAngleThreshold[HowToChooseFixedTwoLines])
{
break;
}
}
vfloat3 temp;
temp = start_points.col(1);
start_points.col(1) = start_points.col(line_id);
start_points.col(line_id) = temp;
temp = end_points.col(1);
end_points.col(1) = end_points.col(line_id);
end_points.col(line_id) = temp;
temp = directions.col(1);
directions.col(1) = directions.col(line_id);
directions.col(line_id) = temp;
temp = points.col(1);
points.col(1) = points.col(line_id);
points.col(line_id) = temp;
lineLenVec[line_id] = lineLenVec[1];
lineLenVec[1] = 0;
// The rotation matrix R_wc is decomposed in way which is slightly different from the description in the paper,
// but the framework is the same.
// R_wc = (Rot') * R * Rot = (Rot') * (Ry(theta) * Rz(phi) * Rx(psi)) * Rot
nc1 = x_cross(start_points.col(1),end_points.col(1));
nc1 = nc1/nc1.norm();
Vw1 = directions.col(1);
Vw1 = Vw1/Vw1.norm();
//the X axis of Model frame
Xm = x_cross(nc1,Vw1);
Xm = Xm/Xm.norm();
//the Y axis of Model frame
Ym = nc1;
//the Z axis of Model frame
Zm = x_cross(Xm,Zm);
Zm = Zm/Zm.norm();
//Rot * [Xm, Ym, Zm] = I.
Rot.col(0) = Xm;
Rot.col(1) = Ym;
Rot.col(2) = Zm;
Rot = Rot.transpose();
//rotate all the vector by Rot.
//nc_bar(:,i) = Rot * nc(:,i)
//Vw_bar(:,i) = Rot * Vw(:,i)
#pragma omp parallel for
for(int i = 0 ; i < n ; i++)
{
vfloat3 nc = x_cross(start_points.col(1),end_points.col(1));
nc = nc/nc.norm();
n_c[i] = nc;
nc_bar[i] = Rot * nc;
Vw_bar[i] = Rot * directions.col(i);
}
//Determine the angle psi, it is the angle between z axis and Vw_bar(:,1).
//The rotation matrix Rx(psi) rotates Vw_bar(:,1) to z axis
float cospsi = (Vw_bar[1])[2];//the angle between z axis and Vw_bar(:,1); cospsi=[0,0,1] * Vw_bar(:,1);.
float sinpsi= sqrt(1 - cospsi*cospsi);
Rx.row(0) = vfloat3(1,0,0);
Rx.row(1) = vfloat3(0,cospsi,-sinpsi);
Rx.row(2) = vfloat3(0,sinpsi,cospsi);
vfloat3 Zaxis = Rx * Vw_bar[1];
if(1-fabs(Zaxis[3]) > 1e-5)
{
Rx = Rx.transpose();
}
//estimate the rotation angle phi by least square residual.
//i.e the rotation matrix Rz(phi)
vfloat3 Vm2 = Rx * Vw_bar[1];
float A2 = Vm2[0];
float B2 = Vm2[1];
float C2 = Vm2[2];
float x2 = (nc_bar[1])[0];
float y2 = (nc_bar[1])[1];
float z2 = (nc_bar[1])[2];
Eigen::PolynomialSolver<double, Eigen::Dynamic> solver;
Eigen::VectorXf coeff(9);
std::vector<float> coef(9,0); //coefficients of equation (7)
Eigen::VectorXf polyDF = VectorXf::Zero(16); //%dF = ployDF(1) * t^15 + ployDF(2) * t^14 + ... + ployDF(15) * t + ployDF(16);
//construct the polynomial F'
#pragma omp parallel for
for(int i = 3 ; i < n ; i++)
{
vfloat3 Vm3 = Rx*Vw_bar[i];
float A3 = Vm3[0];
float B3 = Vm3[1];
float C3 = Vm3[2];
float x3 = (nc_bar[i])[0];
float y3 = (nc_bar[i])[1];
float z3 = (nc_bar[i])[2];
float u11 = -z2*A2*y3*B3 + y2*B2*z3*A3;
float u12 = -y2*A2*z3*B3 + z2*B2*y3*A3;
float u13 = -y2*B2*z3*B3 + z2*B2*y3*B3 + y2*A2*z3*A3 - z2*A2*y3*A3;
float u14 = -y2*B2*x3*C3 + x2*C2*y3*B3;
float u15 = x2*C2*y3*A3 - y2*A2*x3*C3;
float u21 = -x2*A2*y3*B3 + y2*B2*x3*A3;
float u22 = -y2*A2*x3*B3 + x2*B2*y3*A3;
float u23 = x2*B2*y3*B3 - y2*B2*x3*B3 - x2*A2*y3*A3 + y2*A2*x3*A3;
float u24 = y2*B2*z3*C3 - z2*C2*y3*B3;
float u25 = y2*A2*z3*C3 - z2*C2*y3*A3;
float u31 = -x2*A2*z3*A3 + z2*A2*x3*A3;
float u32 = -x2*B2*z3*B3 + z2*B2*x3*B3;
float u33 = x2*A2*z3*B3 - z2*A2*x3*B3 + x2*B2*z3*A3 - z2*B2*x3*A3;
float u34 = z2*A2*z3*C3 + x2*A2*x3*C3 - z2*C2*z3*A3 - x2*C2*x3*A3;
float u35 = -z2*B2*z3*C3 - x2*B2*x3*C3 + z2*C2*z3*B3 + x2*C2*x3*B3;
float u36 = -x2*C2*z3*C3 + z2*C2*x3*C3;
float a4 = u11*u11 + u12*u12 - u13*u13 - 2*u11*u12 + u21*u21 + u22*u22 - u23*u23
-2*u21*u22 - u31*u31 - u32*u32 + u33*u33 + 2*u31*u32;
float a3 =2*(u11*u14 - u13*u15 - u12*u14 + u21*u24 - u23*u25
- u22*u24 - u31*u34 + u33*u35 + u32*u34);
float a2 =-2*u12*u12 + u13*u13 + u14*u14 - u15*u15 + 2*u11*u12 - 2*u22*u22 + u23*u23
+ u24*u24 - u25*u25 +2*u21*u22+ 2*u32*u32 - u33*u33
- u34*u34 + u35*u35 -2*u31*u32- 2*u31*u36 + 2*u32*u36;
float a1 =2*(u12*u14 + u13*u15 + u22*u24 + u23*u25 - u32*u34 - u33*u35 - u34*u36);
float a0 = u12*u12 + u15*u15+ u22*u22 + u25*u25 - u32*u32 - u35*u35 - u36*u36 - 2*u32*u36;
float b3 =2*(u11*u13 - u12*u13 + u21*u23 - u22*u23 - u31*u33 + u32*u33);
float b2 =2*(u11*u15 - u12*u15 + u13*u14 + u21*u25 - u22*u25 + u23*u24 - u31*u35 + u32*u35 - u33*u34);
float b1 =2*(u12*u13 + u14*u15 + u22*u23 + u24*u25 - u32*u33 - u34*u35 - u33*u36);
float b0 =2*(u12*u15 + u22*u25 - u32*u35 - u35*u36);
float d0 = a0*a0 - b0*b0;
float d1 = 2*(a0*a1 - b0*b1);
float d2 = a1*a1 + 2*a0*a2 + b0*b0 - b1*b1 - 2*b0*b2;
float d3 = 2*(a0*a3 + a1*a2 + b0*b1 - b1*b2 - b0*b3);
float d4 = a2*a2 + 2*a0*a4 + 2*a1*a3 + b1*b1 + 2*b0*b2 - b2*b2 - 2*b1*b3;
float d5 = 2*(a1*a4 + a2*a3 + b1*b2 + b0*b3 - b2*b3);
float d6 = a3*a3 + 2*a2*a4 + b2*b2 - b3*b3 + 2*b1*b3;
float d7 = 2*(a3*a4 + b2*b3);
float d8 = a4*a4 + b3*b3;
std::vector<float> v { a4, a3, a2, a1, a0, b3, b2, b1, b0 };
Eigen::VectorXf vp;
vp << a4, a3, a2, a1, a0, b3, b2, b1, b0 ;
//coef = coef + v;
coeff = coeff + vp;
polyDF[0] = polyDF[0] + 8*d8*d8;
polyDF[1] = polyDF[1] + 15* d7*d8;
polyDF[2] = polyDF[2] + 14* d6*d8 + 7*d7*d7;
polyDF[3] = polyDF[3] + 13*(d5*d8 + d6*d7);
polyDF[4] = polyDF[4] + 12*(d4*d8 + d5*d7) + 6*d6*d6;
polyDF[5] = polyDF[5] + 11*(d3*d8 + d4*d7 + d5*d6);
polyDF[6] = polyDF[6] + 10*(d2*d8 + d3*d7 + d4*d6) + 5*d5*d5;
polyDF[7] = polyDF[7] + 9 *(d1*d8 + d2*d7 + d3*d6 + d4*d5);
polyDF[8] = polyDF[8] + 8 *(d1*d7 + d2*d6 + d3*d5) + 4*d4*d4 + 8*d0*d8;
polyDF[9] = polyDF[9] + 7 *(d1*d6 + d2*d5 + d3*d4) + 7*d0*d7;
polyDF[10] = polyDF[10] + 6 *(d1*d5 + d2*d4) + 3*d3*d3 + 6*d0*d6;
polyDF[11] = polyDF[11] + 5 *(d1*d4 + d2*d3)+ 5*d0*d5;
polyDF[12] = polyDF[12] + 4 * d1*d3 + 2*d2*d2 + 4*d0*d4;
polyDF[13] = polyDF[13] + 3 * d1*d2 + 3*d0*d3;
polyDF[14] = polyDF[14] + d1*d1 + 2*d0*d2;
polyDF[15] = polyDF[15] + d0*d1;
}
Eigen::VectorXd coefficientPoly = VectorXd::Zero(16);
for(int j =0; j < 16 ; j++)
{
coefficientPoly[j] = polyDF[15-j];
}
//solve polyDF
solver.compute(coefficientPoly);
const Eigen::PolynomialSolver<double, Eigen::Dynamic>::RootsType & r = solver.roots();
Eigen::VectorXd rs(r.rows());
Eigen::VectorXd is(r.rows());
std::vector<float> min_roots;
for(int j =0;j<r.rows();j++)
{
rs[j] = fabs(r[j].real());
is[j] = fabs(r[j].imag());
}
float maxreal = rs.maxCoeff();
for(int j = 0 ; j < rs.rows() ; j++ )
{
if(is[j]/maxreal <= 0.001)
{
min_roots.push_back(rs[j]);
}
}
std::vector<float> temp_v(15);
std::vector<float> poly(15);
for(int j = 0 ; j < 15 ; j++)
{
temp_v[j] = j+1;
}
for(int j = 0 ; j < 15 ; j++)
{
poly[j] = polyDF[j]*temp_v[j];
}
Eigen::Matrix<double,16,1> polynomial;
Eigen::VectorXd evaluation(min_roots.size());
for( int j = 0; j < min_roots.size(); j++ )
{
evaluation[j] = poly_eval( polynomial, min_roots[j] );
}
std::vector<float> minRoots;
for( int j = 0; j < min_roots.size(); j++ )
{
if(!evaluation[j]<=0)
{
minRoots.push_back(min_roots[j]);
}
}
if(minRoots.size()==0)
{
cout << "No solution" << endl;
return;
}
int numOfRoots = minRoots.size();
//for each minimum, we try to find a solution of the camera pose, then
//choose the one with the least reprojection residual as the optimum of the solution.
float minimalReprojectionError = 100;
// In general, there are two solutions which yields small re-projection error
// or condition error:"n_c * R_wc * V_w=0". One of the solution transforms the
// world scene behind the camera center, the other solution transforms the world
// scene in front of camera center. While only the latter one is correct.
// This can easily be checked by verifying their Z coordinates in the camera frame.
// P_c(Z) must be larger than 0 if it's in front of the camera.
for(int rootId = 0 ; rootId < numOfRoots ; rootId++)
{
float cosphi = minRoots[rootId];
float sign1 = sign_of_number(coeff[0] * pow(cosphi,4)
+ coeff[1] * pow(cosphi,3) + coeff[2] * pow(cosphi,2)
+ coeff[3] * cosphi + coeff[4]);
float sign2 = sign_of_number(coeff[5] * pow(cosphi,3)
+ coeff[6] * pow(cosphi,2) + coeff[7] * cosphi + coeff[8]);
float sinphi= -sign1*sign2*sqrt(abs(1-cosphi*cosphi));
Eigen::MatrixXf Rz(3,3);
Rz.row(0) = vfloat3(cosphi,-sinphi,0);
Rz.row(1) = vfloat3(sinphi,cosphi,0);
Rz.row(2) = vfloat3(0,0,1);
//now, according to Sec4.3, we estimate the rotation angle theta
//and the translation vector at a time.
Eigen::MatrixXf RzRxRot(3,3);
RzRxRot = Rz*Rx*Rot;
//According to the fact that n_i^C should be orthogonal to Pi^c and Vi^c, we
//have: scalarproduct(Vi^c, ni^c) = 0 and scalarproduct(Pi^c, ni^c) = 0.
//where Vi^c = Rwc * Vi^w, Pi^c = Rwc *(Pi^w - pos_cw) = Rwc * Pi^w - pos;
//Using the above two constraints to construct linear equation system Mat about
//[costheta, sintheta, tx, ty, tz, 1].
Eigen::MatrixXf Matrice(2*n-1,6);
#pragma omp parallel for
for(int i = 0 ; i < n ; i++)
{
float nxi = (nc_bar[i])[0];
float nyi = (nc_bar[i])[1];
float nzi = (nc_bar[i])[2];
Eigen::VectorXf Vm(3);
Vm = RzRxRot * directions.col(i);
float Vxi = Vm[0];
float Vyi = Vm[1];
float Vzi = Vm[2];
Eigen::VectorXf Pm(3);
Pm = RzRxRot * points.col(i);
float Pxi = Pm(1);
float Pyi = Pm(2);
float Pzi = Pm(3);
//apply the constraint scalarproduct(Vi^c, ni^c) = 0
//if i=1, then scalarproduct(Vi^c, ni^c) always be 0
if(i>1)
{
Matrice(2*i-3, 0) = nxi * Vxi + nzi * Vzi;
Matrice(2*i-3, 1) = nxi * Vzi - nzi * Vxi;
Matrice(2*i-3, 5) = nyi * Vyi;
}
//apply the constraint scalarproduct(Pi^c, ni^c) = 0
Matrice(2*i-2, 0) = nxi * Pxi + nzi * Pzi;
Matrice(2*i-2, 1) = nxi * Pzi - nzi * Pxi;
Matrice(2*i-2, 2) = -nxi;
Matrice(2*i-2, 3) = -nyi;
Matrice(2*i-2, 4) = -nzi;
Matrice(2*i-2, 5) = nyi * Pyi;
}
//solve the linear system Mat * [costheta, sintheta, tx, ty, tz, 1]' = 0 using SVD,
JacobiSVD<MatrixXf> svd(Matrice, ComputeThinU | ComputeThinV);
Eigen::MatrixXf VMat = svd.matrixV();
Eigen::VectorXf vec(2*n-1);
//the last column of Vmat;
vec = VMat.col(5);
//the condition that the last element of vec should be 1.
vec = vec/vec[5];
//the condition costheta^2+sintheta^2 = 1;
float normalizeTheta = 1/sqrt(vec[0]*vec[1]+vec[1]*vec[1]);
float costheta = vec[0]*normalizeTheta;
float sintheta = vec[1]*normalizeTheta;
Eigen::MatrixXf Ry(3,3);
Ry << costheta, 0, sintheta , 0, 1, 0 , -sintheta, 0, costheta;
//now, we get the rotation matrix rot_wc and translation pos_wc
Eigen::MatrixXf rot_wc(3,3);
rot_wc = (Rot.transpose()) * (Ry * Rz * Rx) * Rot;
Eigen::VectorXf pos_wc(3);
pos_wc = - Rot.transpose() * vec.segment(2,4);
//now normalize the camera pose by 3D alignment. We first translate the points
//on line in the world frame Pw to points in the camera frame Pc. Then we project
//Pc onto the line interpretation plane as Pc_new. So we could call the point
//alignment algorithm to normalize the camera by aligning Pc_new and Pw.
//In order to improve the accuracy of the aligment step, we choose two points for each
//lines. The first point is Pwi, the second point is the closest point on line i to camera center.
//(Pw2i = Pwi - (Pwi'*Vwi)*Vwi.)
Eigen::MatrixXf Pw2(3,n);
Pw2.setZero();
Eigen::MatrixXf Pc_new(3,n);
Pc_new.setZero();
Eigen::MatrixXf Pc2_new(3,n);
Pc2_new.setZero();
for(int i = 0 ; i < n ; i++)
{
vfloat3 nci = n_c[i];
vfloat3 Pwi = points.col(i);
vfloat3 Vwi = directions.col(i);
//first point on line i
vfloat3 Pci;
Pci = rot_wc * Pwi + pos_wc;
Pc_new.col(i) = Pci - (Pci.transpose() * nci) * nci;
//second point is the closest point on line i to camera center.
vfloat3 Pw2i;
Pw2i = Pwi - (Pwi.transpose() * Vwi) * Vwi;
Pw2.col(i) = Pw2i;
vfloat3 Pc2i;
Pc2i = rot_wc * Pw2i + pos_wc;
Pc2_new.col(i) = Pc2i - ( Pc2i.transpose() * nci ) * nci;
}
MatrixXf XXc(Pc_new.rows(), Pc_new.cols()+Pc2_new.cols());
XXc << Pc_new, Pc2_new;
MatrixXf XXw(points.rows(), points.cols()+Pw2.cols());
XXw << points, Pw2;
int nm = points.cols()+Pw2.cols();
cal_campose(XXc,XXw,nm,rot_wc,pos_wc);
pos_cw = -rot_wc.transpose() * pos_wc;
//check the condition n_c^T * rot_wc * V_w = 0;
float conditionErr = 0;
for(int i =0 ; i < n ; i++)
{
float val = ( (n_c[i]).transpose() * rot_wc * directions.col(i) );
conditionErr = conditionErr + val*val;
}
if(conditionErr/n < condition_err_threshold || HowToChooseFixedTwoLines ==3)
{
//check whether the world scene is in front of the camera.
int numLineInFrontofCamera = 0;
if(HowToChooseFixedTwoLines<3)
{
for(int i = 0 ; i < n ; i++)
{
vfloat3 P_c = rot_wc * (points.col(i) - pos_cw);
if(P_c[2]>0)
{
numLineInFrontofCamera++;
}
}
}
else
{
numLineInFrontofCamera = n;
}
if(numLineInFrontofCamera > 0.5*n)
{
//most of the lines are in front of camera, then check the reprojection error.
int reprojectionError = 0;
for(int i =0; i < n ; i++)
{
//line projection function
vfloat3 nc = rot_wc * x_cross(points.col(i) - pos_cw , directions.col(i));
float h1 = nc.transpose() * start_points.col(i);
float h2 = nc.transpose() * end_points.col(i);
float lineLen = (start_points.col(i) - end_points.col(i)).norm()/3;
reprojectionError += lineLen * (h1*h1 + h1*h2 + h2*h2) / (nc[0]*nc[0]+nc[1]*nc[1]);
}
if(reprojectionError < minimalReprojectionError)
{
optimumrot_cw = rot_wc.transpose();
optimumpos_cw = pos_cw;
minimalReprojectionError = reprojectionError;
}
}
}
}
if(optimumrot_cw.rows()>0)
{
break;
}
}
pos_cw = optimumpos_cw;
rot_cw = optimumrot_cw;
}
int tensor_eval( int tag, int m, int n, int d, int p,
double* x, double **tensor, double **S ) {
static int bd,dim;
static int dold,pold;
static struct item *coeff_list;
int i,j,k,dimten,ctr;
int **jm, jmbd=0;
int *it = (int*) malloc(d*sizeof(int));
double *y = (double*) malloc(m*sizeof(double));
double*** X;
double*** Y;
struct item *ptr[10];
int rc = 3;
dimten=binomi(p+d,d);
for (i=0; i<m; i++)
for (j=0; j<dimten; j++)
tensor[i][j] = 0;
if (d == 0) {
MINDEC(rc,zos_forward(1,m,n,0,x,y));
} else {
if ((d != dold) || (p != pold)) {
if (pold) {
dim = binomi(pold+dold-1,dold);
freecoefflist(dim,coeff_list);
free((char*) coeff_list);
}
dim = binomi(p+d-1,d);
if (dim < 10)
bd = dim;
else
bd = 10;
coeff_list = (struct item *) malloc(sizeof(struct item)*dim);
coeff(p,d, coeff_list);
dold = d;
pold = p;
}
jmbd = bd;
jm = (int **) malloc(jmbd*sizeof(int*));
for (i=0; i<jmbd; i++)
jm[i] = (int *) malloc(p*sizeof(int));
if (d == 1) {
X = myalloc3(1,n,bd);
Y = myalloc3(1,m,bd);
ctr = 0;
it[0] = 0;
for (i=0; i<dim; i++) /* sum over all multiindices jm with |jm| = d */
{ it[0] = it[0]+1;
convert(p,d,it,jm[ctr]);
ptr[ctr] = &coeff_list[i];
if (ctr < bd-1)
ctr += 1;
else {
multma2vec2(n,p,bd,X[0],S,jm);
MINDEC(rc,fov_forward(tag,m,n,bd,x,X[0],y,Y[0]));
for (k=0; k<bd; k++)
do {
for (j=0; j<m; j++)
tensor[j][ptr[k]->a] += Y[0][j][k]*ptr[k]->c;
ptr[k] = ptr[k]->next;
} while (ptr[k] != NULL);
if (dim-i < bd)
bd = dim-i-1;
ctr = 0;
}
}
} else {
X = myalloc3(n,bd,d);
Y = myalloc3(m,bd,d);
ctr = 0;
for (i=0; i<d-1; i++)
it[i] = 1;
it[d-1] = 0;
for (i=0; i<dim; i++) /* sum over all multiindices jm with |jm| = d */
{ it[d-1] = it[d-1]+1;
for (j=d-2; j>=0; j--)
it[j] = it[j] + it[j+1]/(p+1);
for (j=1; j<d; j++)
if (it[j] > p)
it[j] = it[j-1];
convert(p,d,it,jm[ctr]);
ptr[ctr] = &coeff_list[i];
if (ctr < bd-1)
ctr += 1;
else {
multma3vec2(n,p,d,bd,X,S,jm);
MINDEC(rc,hov_forward(tag,m,n,d,bd,x,X,y,Y));
for (k=0; k<bd; k++)
do {
for (j=0; j<m; j++)
tensor[j][ptr[k]->a] += Y[j][k][ptr[k]->b-1]*ptr[k]->c;
ptr[k] = ptr[k]->next;
} while (ptr[k] != NULL);
if (dim-i < bd)
bd = dim-i-1;
ctr = 0;
}
}
}
for (i=0; i<jmbd; i++)
free((char*) *(jm+i));
free((char*) jm);
free((char*) **X);
free((char*) *X);
free((char*) X);
free((char*) **Y);
free((char*) *Y);
free((char*) Y);
}
for(i=0;i<m;i++)
tensor[i][0] = y[i];
bd = jmbd;
free((char*) y);
free((char*) it);
return rc;
}
int inverse_tensor_eval( int tag, int n, int d, int p,
double *x, double **tensor, double** S ) {
static int dim;
static int dold,pold;
static struct item *coeff_list;
int i,j,dimten;
int *it = (int*) malloc(d*sizeof(int));
double** X;
double** Y;
int *jm;
double *y = (double*) malloc(n*sizeof(double));
struct item *ptr;
int rc = 3;
dimten=binomi(p+d,d);
for(i=0;i<n;i++)
for(j=0;j<dimten;j++)
tensor[i][j] = 0;
MINDEC(rc,zos_forward(1,n,n,0,x,y));
if (d > 0) {
if ((d != dold) || (p != pold)) {
if (pold) { /* olvo 980728 */
dim = binomi(pold+dold-1,dold);
freecoefflist(dim,coeff_list);
free((char*) coeff_list);
}
dim = binomi(p+d-1,d);
coeff_list = (struct item *) malloc(sizeof(struct item)*dim);
coeff(p,d, coeff_list);
dold = d;
pold = p;
}
jm = (int *)malloc(sizeof(int)*p);
X = myalloc2(n,d+1);
Y = myalloc2(n,d+1);
for (i=0; i<n; i++) {
X[i][0] = x[i];
for (j=1; j<d; j++)
X[i][j] = 0;
Y[i][0] = y[i];
}
if (d == 1) {
it[0] = 0;
for (i=0; i<dim; i++) /* sum over all multiindices jm with |jm| = d */
{ it[0] = it[0]+1;
convert(p,d,it,jm);
ptr = &coeff_list[i];
multma2vec1(n,p,d,Y,S,jm);
MINDEC(rc,inverse_Taylor_prop(tag,n,d,Y,X));
if (rc == -3)
return -3;
do {
for(j=0;j<n;j++)
tensor[j][ptr->a] += X[j][ptr->b]*ptr->c;
ptr = ptr->next;
} while (ptr != NULL);
}
} else {
for (i=0; i<d-1; i++)
it[i] = 1;
it[d-1] = 0;
for (i=0; i<dim; i++) /* sum over all multiindices jm with |jm| = d */
{ it[d-1] = it[d-1]+1;
for (j=d-2; j>=0; j--)
it[j] = it[j] + it[j+1]/(p+1);
for (j=1; j<d; j++)
if (it[j] > p) it[j] = it[j-1];
convert(p,d,it,jm);
multma2vec1(n,p,d,Y,S,jm); /* Store S*jm in Y */
MINDEC(rc,inverse_Taylor_prop(tag,n,d,Y,X));
if (rc == -3)
return -3;
ptr = &coeff_list[i];
do {
for(j=0;j<n;j++)
tensor[j][ptr->a] += X[j][ptr->b]*ptr->c;
ptr = ptr->next;
} while (ptr != NULL);
}
}
free((char*) jm);
free((char*) *X);
free((char*) X);
free((char*) *Y);
free((char*) Y);
}
for(i=0;i<n;i++)
tensor[i][0] = x[i];
free((char*) y);
free((char*) it);
return rc;
}
/*
* Find the ground in the environment.
* Params[in/out]: the cloud, the coeff of the planes, the indices, the ground cloud, the hull cloud
*/
bool Segmentation::findGround(pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &cloud, MainPlane &mp)
{
pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr indices(new pcl::PointIndices);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr ground(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr hullGround(new pcl::PointCloud<pcl::PointXYZRGBA>);
// Create the segmentation object
pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE); // We search a plane perpendicular to the y
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.030); //0.25 / 35
seg.setAxis(_axis); // Axis Y 0, -1, 0
seg.setEpsAngle(30.0f * (M_PI/180.0f)); // 50 degrees of error
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
seg.setInputCloud (cloud);
seg.segment (*indices, *coeff);
mp.setCoefficients(coeff);
//mp.setIndices(indices);
if (indices->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model (Ground).");
return false;
}
else
{
extract.setInputCloud(cloud);
extract.setIndices(indices);
extract.setNegative(false);
extract.filter(*ground);
mp.setPlaneCloud(ground);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr concaveHull(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr plane(new pcl::PointCloud<pcl::PointXYZRGBA>);
// Copy the points of the plane to a new cloud.
pcl::ExtractIndices<pcl::PointXYZRGBA> extractHull;
extractHull.setInputCloud(cloud);
extractHull.setIndices(indices);
extractHull.filter(*plane);
pcl::ConcaveHull<pcl::PointXYZRGBA> chull;
chull.setInputCloud (plane);
chull.setAlpha (0.1);
chull.reconstruct (*concaveHull);
mp.setHull(concaveHull);
//vectorCloudinliers.push_back(convexHull);
extract.setNegative(true);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_f (new pcl::PointCloud<pcl::PointXYZRGBA>);
extract.filter(*cloud_f);
cloud.swap(cloud_f);
return true;
}
}
void Segmentation::ransac(std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &clusters, Eigen::Vector3f axis, std::vector < pcl::PointCloud<pcl::PointXYZRGBA>::Ptr > &vectorCloudInliers )
{
vectorCloudInliers.clear();
for(unsigned int i = 0; i < clusters.size(); i++)
{
pcl::ModelCoefficients::Ptr coeff(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr indices(new pcl::PointIndices);
pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud_(new pcl::PointCloud<pcl::PointXYZRGBA>);
//pcl::PointCloud<pcl::PointXYZRGBA>::Ptr hullGround(new pcl::PointCloud<pcl::PointXYZRGBA>);
pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> ne;
pcl::SACSegmentationFromNormals<pcl::PointXYZRGBA, pcl::Normal> seg;
pcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ());
pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);
// Estimate Normals
ne.setSearchMethod(tree);
ne.setInputCloud(clusters[i]);
ne.setKSearch(50);
ne.compute(*cloud_normals);
// Create the segmentation object
//pcl::SACSegmentation<pcl::PointXYZRGBA> seg;
// Optional
seg.setOptimizeCoefficients (true);
// Mandatory
//seg.setModelType (pcl::SACMODEL_PERPENDICULAR_PLANE); // We search a plane perpendicular to the y
seg.setModelType (pcl::SACMODEL_NORMAL_PLANE);// We search a plane perpendicular to the y
seg.setNormalDistanceWeight(0.005);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setDistanceThreshold (0.020); //0.25 / 35
//seg.setAxis(axis); // Axis Y 0, -1, 0
seg.setEpsAngle(20.0f * (M_PI/180.0f)); // 50 degrees of error
pcl::ExtractIndices<pcl::PointXYZRGBA> extract;
seg.setInputCloud (clusters[i]);
seg.setInputNormals(cloud_normals);
seg.segment (*indices, *coeff);
if(normalDifferenceError(coeff))
{
std::cout << "coeff" << coeff->values[0] << " " << coeff->values[1] << " " << coeff->values[2] << std::endl;
if (indices->indices.size () == 0)
{
PCL_ERROR ("Could not estimate a planar model (Ground).");
//return false;
}
else
{
extract.setInputCloud(clusters[i]);
extract.setIndices(indices);
extract.setNegative(false);
extract.filter(*cloud_);
vectorCloudInliers.push_back(cloud_);
//return true;
}
}
}
}
void spectralResidualSaliencyMap(
const Mat_<float> &image,
Mat_<float> &saliencyMap,
int avgFilterSize,
float gaussianSigma,
int downSamplingMaxDim) {
// down sampling of the image to a size smaller than 64x64
Size newSize = image.cols > image.rows ?
Size(downSamplingMaxDim, image.rows * downSamplingMaxDim / image.cols) :
Size(image.cols * downSamplingMaxDim / image.rows, downSamplingMaxDim);
Mat downSampled8b;
resize(image, downSampled8b, newSize);
Mat downSampled = Mat_<float>(downSampled8b) / 255.;
imshow("downSampled", downSampled);
vector<Mat> planes;
planes.push_back(downSampled);
planes.push_back(Mat::zeros(newSize, CV_32FC1));
Mat downSampledComplex(newSize, CV_32FC2);
merge(planes, downSampledComplex);
// compute its residual
// first compute its spectrum
Mat fourierSpectrum;
cout<<"computing residual"<<endl;
dft(downSampledComplex, fourierSpectrum, DFT_COMPLEX_OUTPUT);
Mat parts[2];
split(fourierSpectrum, parts);
// apply log scaling
// first normalize the range of values to nonnegative values
double spectrumMin;
minMaxLoc(parts[0], &spectrumMin);
parts[0] = parts[0] - spectrumMin + 1;
Mat logSpectrum;
log(parts[0], logSpectrum);
showScaled("spectrum", parts[0]);
showScaled("logSpectrum", logSpectrum);
Mat logSpectrumParts[] = {logSpectrum, parts[1]};
Mat fullLogSpectrum, invDFTLog;
merge(logSpectrumParts, 2, fullLogSpectrum);
idft(fullLogSpectrum, invDFTLog, DFT_REAL_OUTPUT);
showScaled("invDFTLog", invDFTLog);
Mat filteredSpectrum;
Mat avgFilter =
Mat::ones(avgFilterSize, avgFilterSize, CV_32FC1) / (avgFilterSize * avgFilterSize);
filter2D(logSpectrum, filteredSpectrum, -1, avgFilter);
Mat residual = logSpectrum - filteredSpectrum;
// turn the spectrum back to an image, with some filtering
Mat exponentiated(residual.rows, residual.cols, CV_32FC2);
Mat unfilteredOutput(downSampled.rows, downSampled.cols, CV_32FC2);
Mat squared;
// exponentiating the coefficients manually, because opencv sucks at complex
// arithmetic
for (int i = 0; i < residual.rows; i++) {
for (int j = 0; j < residual.cols; j++) {
complex<float> coeff(residual.at<float>(i,j), parts[1].at<float>(i,j));
complex<float> expCoeff = exp(coeff);
exponentiated.at<Vec2f>(i,j)[0] = expCoeff.real();
exponentiated.at<Vec2f>(i,j)[1] = expCoeff.imag();
}
}
cout<<"running inverse dft"<<endl;
idft(exponentiated, unfilteredOutput, DFT_SCALE);
Mat unfilteredParts[2];
split(unfilteredOutput, unfilteredParts);
pow(unfilteredParts[0], 2, squared);
GaussianBlur(squared, saliencyMap, Size(0,0), gaussianSigma);
}
