int Metric::cosine(vector<Coefficient>& table,
const map<int, map<int, double> >& profile)
{
map<int, map<int, double> >::const_iterator iter;
double sig_x_2 = 0;
double sig_y_2 = 0;
double prod_xy = 0;
int cooc = 0;
for (iter=profile.begin(); iter!=profile.end(); iter++) {
map<int, double>::const_iterator xiter;
sig_x_2 = 0;
for (xiter=iter->second.begin(); xiter!=iter->second.end(); xiter++) {
sig_x_2 += xiter->second*xiter->second;
}
map<int, map<int, double> >::const_iterator jter = iter;
for (++jter; jter != profile.end(); jter++) {
map<int, double>::const_iterator yiter;
sig_y_2 = 0;
prod_xy = 0;
for (yiter=jter->second.begin(); yiter!=jter->second.end(); yiter++) {
sig_y_2 += yiter->second*yiter->second;
if ( iter->second.find(yiter->first) != iter->second.end() ) {
prod_xy += yiter->second * iter->second.find(yiter->first)->second;
}
}
double cor = prod_xy / (sqrt(sig_x_2) * sqrt(sig_y_2));
cooc = 0;
map<int, double>::const_iterator it;
if ( (it=iter->second.find(jter->first)) != iter->second.end() ) {
cooc = (int) it->second;
}
Coefficient coef(iter->first,jter->first, cooc, cor);
table.push_back(coef);
}
}
return 0;
}
template <class T> void ossimShiftFilter::fillTile(T /* dummy */,
const ossimImageData* inputTile,
ossimImageData* outputTile) const
{
const double BANDS = inputTile->getNumberOfBands();
const ossim_uint32 SPB = inputTile->getSizePerBand();
std::vector<double> inNull(BANDS);
std::vector<double> inMin(BANDS);
std::vector<double> inMax(BANDS);
std::vector<double> coef(BANDS);
ossim_uint32 band = 0;
for( ; band < BANDS; ++band )
{
inNull[band] = inputTile->getNullPix(band);
inMin[band] = inputTile->getMinPix(band);
inMax[band] = inputTile->getMaxPix(band);
coef[band] = (m_max-m_min)/(inMax[band]-inMin[band]);
}
double pix = 0;
for( band = 0; band < BANDS; ++band )
{
const T* inBuf = static_cast<const T*>(inputTile->getBuf(band));
T* outBuf = static_cast<T*>(outputTile->getBuf(band));
for ( ossim_uint32 i = 0; i < SPB; ++i )
{
pix = inBuf[i];
if ( pix == inNull[band] )
{
pix = m_null;
}
else
{
// Shift and multiply:
pix = m_min + (pix - inMin[band]) * coef[band];
// Range check:
pix = pix <= m_max ? (pix >= m_min ? pix : m_min) : m_max;
}
outBuf[i] = static_cast<T>(pix);
}
}
outputTile->validate();
}
//Evaluate right continuous surface data.
//Evaluate all positional data, 1st and 2nd derivatives.
void MGSBRep::eval_all(
double u, double v, // Parameter value of the surface.
MGPosition& f, // Positional data.
MGVector& fu, // df(u,v)/du
MGVector& fv, // df/dv
MGVector& fuv, // d**2f/(du*dv)
MGVector& fuu, // d**2f/(du**2)
MGVector& fvv // d**2f/(dv**2)
) const
{
size_t ku=order_u(), kv=order_v();
size_t ku2=ku+ku, kv2=kv+kv;
double *ucoef=new double[ku*3], *vcoef=new double[kv*3];
int bdum1=bdim_u()-1, bdvm1=bdim_v()-1;
int uid=m_uknot.eval_coef(u,ucoef,0);
int vid=m_vknot.eval_coef(v,vcoef,0);
m_uknot.eval_coef(u,ucoef+ku,1); m_uknot.eval_coef(u,ucoef+ku2,2);
m_vknot.eval_coef(v,vcoef+kv,1); m_vknot.eval_coef(v,vcoef+kv2,2);
double s,su,suu,c,vj,vj1; size_t i,j,k,dim=sdim();
int ii,jj;
MGPosition p(dim);
MGVector pu(dim),pv(dim),puv(dim),puu(dim),pvv(dim);
for(k=0; k<dim; k++){
p(k)=0.0;pu.set(k)=0.0;pv.set(k)=0.0;puv.set(k)=0.0;
puu.set(k)=0.0;pvv.set(k)=0.0;
for(j=0; j<kv; j++){
s=su=suu=0.0;
jj=vid+j;
for(i=0; i<ku; i++){
ii=uid+i;
c=coef(ii,jj,k);
s=s+ucoef[i]*c;
su=su+ucoef[i+ku]*c;
suu=suu+ucoef[i+ku2]*c;
}
vj=vcoef[j]; vj1=vcoef[j+kv];
p(k)=p(k)+vj*s;
pu.set(k)=pu.ref(k)+vj*su;
pv.set(k)=pv.ref(k)+vj1*s;
puv.set(k)=puv.ref(k)+vj1*su;
puu.set(k)=puu.ref(k)+vj*suu;
pvv.set(k)=pvv.ref(k)+vcoef[j+kv2]*s;
}
}
f=p;fu=pu;fv=pv;fuv=puv;fuu=puu;fvv=pvv;
delete[] ucoef; delete[] vcoef;
}
MGPPRep::MGPPRep(unsigned order, const MGPPRep& pp1)
//Constructor to change order of original PP-Representation.
//New order may be greater or less than the original one. However,
//if new one is less than the original, PP-Rep constructed may not
//be able to hold the same shape.
:m_order(order)
,m_nbreak(pp1.m_nbreak)
,m_sdim(pp1.m_sdim)
,m_break_point(pp1.m_break_point)
,m_coef(new double[m_order*m_nbreak*m_sdim])
{
size_t i,j,k;
for(k=0; k<m_sdim; k++)
for(j=0; j<m_nbreak-1; j++)
for(i=0; i<m_order; i++)
coef(i,j,k)=pp1.ref(i,j,k);
}
//Change size. Change of sdim not allowed.
//Stored data so far will be guarateed to hold in the same id of coef(i,j,k).
void MGPPRep::reshape(size_t nbreak){
if(nbreak==m_nbreak) return ;
double* data=new double[m_order*nbreak*m_sdim];
//Reshape of pp coef.
size_t nb=nbreak; if(nb>m_nbreak) nb=m_nbreak;
for(size_t k=0; k<m_sdim; k++){
for(size_t j=0; j<nb; j++){
for(unsigned i=0; i<m_order ; i++){
data[i+m_order*j+m_order*nbreak*k]=coef(i,j,k);
}
}
}
delete[] m_coef; m_coef=data;
m_break_point.reshape(nbreak); //Reshape of break poits.
m_nbreak=nbreak;
}
int main(void)
{
int n;
for (n = 0; n < 10; n++) {
coef(n);
printf("(x-1)^%d = ", n);
show(n);
putchar('\n');
}
printf("\nprimes (never mind the 1):");
for (n = 1; n <= 63; n++)
if (is_prime(n))
printf(" %d", n);
putchar('\n');
return 0;
}
int bush(GF & gf, bclib::matrix<int> & A, int str, int ncol)
{
int q = gf.q;
std::vector<int> coef(str);
// bushcheck throws if it fails
bushcheck(q, str, ncol);
for (size_t i = 0; i < static_cast<size_t>(primes::ipow(q, str)); i++)
{
itopoly(static_cast<int>(i), q, str - 1, coef);
A(i, static_cast<size_t>(0)) = coef[static_cast<size_t>(str) - 1];
for (size_t j = 0; j < static_cast<size_t>(ncol) - 1; j++)
{
polyeval(gf, str - 1, coef, static_cast<int>(j), &(A(i, 1 + j)));
}
}
return SUCCESS_CHECK;
}
//Evaluate all of derivative data (d(i+j)f(u,v))/(du**i*dv**j),
//for 0<=i<=ndu and 0<=j<=ndv.
void MGSBRep::eval_all(
double u, double v, // Parameter value of the surface.
size_t ndu, //Order of Derivative along u.
size_t ndv, //Order of Derivative along v.
double* deriv //Output. (d(i+j)f(u,v))/(du**i*dv**j) in
//deriv[r+j*dim+i*(ndv+1)*dim] for 0<=r<dim=sdim().
//for 0<=i<=ndu and 0<=j<=ndv.
//deriv is an array of deriv[ndu+1][ndv+1][r],
//(d(i+j)f(u,v))/(du**i*dv**j) is returned in deriv[i][j][r].
) const{
size_t i,j,jj,r, dim=sdim();
size_t ider,jder;
const unsigned ku=order_u(), kv=order_v();
size_t kundu=ku*(ndu+1), kvndv=kv*(ndv+1);
double cu[30],cv[30]; double *cup=cu, *cvp=cv;
if(kundu>30) cup=new double[kundu]; //Done to save "new".
if(kvndv>30) cvp=new double[kvndv]; //Done to save "new".
int idu,idv;
for(ider=0; ider<=ndu; ider++) idu=m_uknot.eval_coef(u,cup+ider*ku,ider);
for(jder=0; jder<=ndv; jder++) idv=m_vknot.eval_coef(v,cvp+jder*kv,jder);
double coefall,coefu;
for(ider=0; ider<=ndu;ider++){
size_t iderndv1dim=ider*(ndv+1)*dim;
for(jder=0; jder<=ndv; jder++){
size_t jderdim=jder*dim;
for(r=0; r<dim; r++){
coefall=0.;
for(j=0;j<kv;j++){
coefu=0.;
jj=idv+j;
for(i=0;i<ku;i++)
coefu=coefu+coef(idu+i,jj,r)*cup[i+ider*ku];
coefall=coefall+coefu*cvp[j+jder*kv];
}
deriv[r+jderdim+iderndv1dim]=coefall;
}
}
}
if(kundu>30) delete[] cup;
if(kvndv>30) delete[] cvp;
}
int sample()
{
LL n = 3;
int deg = 3;
Poly W(deg), coef(deg);
coef[2] = 1; coef[1] = 2;
W[0] = 3; W[1] = 4;
// W_i = 2 * W_{i-1} + 1 * W_{i - 2}
// W_0 = 3, W_1 = 4, W_2 = 11, W_3 = 25 ...
Poly P(deg);
P[1] = 1;
Poly A = power(P, n, coef);
int ret = 0;
for (int i = 0; i < deg; ++ i)
add(ret, mul(W[i], coef[i]));
return ret;
}
// append local profile to MNI space profile
void appendpf_local2std(string coordfile_std2local) {
ifstream coordstream(coordfile_std2local);
floatVector coord(3);
vector<floatVector> coord_map_std2local;
for (int i=0; i<3; i++)
coordstream >> coord[i];
while(coordstream.good()) {
coord_map_std2local.push_back(coord);
for (int i=0; i<3; i++)
coordstream >> coord[i];
}
assert(coord_map_std2local.size()==conn_profile().size());
floatVector append;
intVector start(3);
floatVector coef(3);
intVector tmpCoord(3);
int dim_profile = (conn_profile().begin()->second).size();
auto it = conn_profile().begin();
for (int i=0; i<conn_profile().size(); i++, it++) {
for (int k=0; k<3; k++) {
start[k]=(int)coord_map_std2local[i][k];
coef[k] = coord_map_std2local[i][k]-start[k];
}
for (int z=0; z<2; z++) { // Note: we assume that all points are within the interiors, therefore exactly 8 neighbors
float fracZ = (1-coef[2])*(1-z)+coef[2]*z;
for (int y=0; y<2; y++) {
float fracZY=fracZ * ( (1-coef[1])*(1-y)+coef[1]*y );
for (int x=0; x<2; x++) {
float fracZYX=fracZY * ( (1-coef[0])*(1-x)+coef[0]*x );
tmpCoord[0]=start[0]+x; tmpCoord[1]=start[1]+y; tmpCoord[2]=start[2]+z;
if (conn_profile_local().count(tmpCoord)) {
for (int k=0; k<dim_profile; k++)
(it->second)[k] += fracZYX * conn_profile_local()[tmpCoord][k];
}
}
}
}
}
}
MGPPRep::MGPPRep(const MGLBRep& lbrep)
//Constructor to convert from Line B-Representation.
:m_order(lbrep.order()), //Order
m_nbreak(lbrep.bdim()-m_order+2), //Num of Break points
m_sdim(lbrep.sdim()), //Space dimension
m_break_point(m_nbreak), //Break point area
m_coef(new double[m_order*m_nbreak*m_sdim]) //Coef area
{
const size_t irc=lbrep.line_bcoef().capacity();
double* work=new double[m_order*m_order*m_sdim]; //Work array for BLCBP
size_t nbdim= lbrep.bdim();
const double* knotp= lbrep.knot_data();
int nbreak;
double* breakp=&(m_break_point(0));
blcbpn_(m_order,nbdim,knotp,lbrep.coef_data(),irc,
m_sdim,m_nbreak,work,breakp,&coef(0,0,0),&nbreak);
delete[] work;
nbreak+=1; //Since ouput nbreak of BLCBP is interval number.
if(nbreak<int(m_nbreak))
reshape(nbreak);
}
void Energy::read_parameters()
{
ifstream parameters;
parameters.open("energy.cfg");
if(! parameters.is_open())
{
cout << "Error:File 'energy.cfg' could not be found\n";
exit(0);
}
printf("Setting parameters\n");
string dummy;
la::vector<double> coef;
while (!parameters.eof())
{
parameters >> dummy;
if(dummy == "MODE:")
parameters >> mode;
if(dummy == "DATASET:")
parameters >> dataset;
if(dummy == "COEFFICIENTS:")
{
parameters >> dummy;
while(dummy != "end")
{
coef.resize(coef.size()+1,true);
coef(coef.size()-1) = (double) atof(dummy.c_str());
parameters >> dummy;
}
}
} //end read file
CzWINDOWEDFIR::CzWINDOWEDFIR()
{
int _LPcl;
float _LPcllen = (float)(1L<<WFIR_FRACBITS); // number of precalculated lines for 0..1 (-1..0)
float _LNorm = 1.0f / (float)(2.0f * _LPcllen);
float _LCut = WFIR_CUTOFF;
float _LScale = (float)WFIR_QUANTSCALE;
for( _LPcl=0;_LPcl<WFIR_LUTLEN;_LPcl++ )
{
float _LGain,_LCoefs[WFIR_WIDTH];
float _LOfs = ((float)_LPcl-_LPcllen)*_LNorm;
int _LCc,_LIdx = _LPcl<<WFIR_LOG2WIDTH;
for( _LCc=0,_LGain=0.0f;_LCc<WFIR_WIDTH;_LCc++ )
{ _LGain += (_LCoefs[_LCc] = coef( _LCc, _LOfs, _LCut, WFIR_WIDTH, WFIR_TYPE ));
}
_LGain = 1.0f/_LGain;
for( _LCc=0;_LCc<WFIR_WIDTH;_LCc++ )
{ float _LCoef = (float)floor( 0.5 + _LScale*_LCoefs[_LCc]*_LGain );
lut[_LIdx+_LCc] = (signed short)( (_LCoef<-_LScale)?-_LScale:((_LCoef>_LScale)?_LScale:_LCoef) );
}
}
}
void GlmModel::set_included_coefficients(const Vector &beta,
const Selector &inc) {
coef().set_included_coefficients(beta, inc);
}
void GlmModel::set_included_coefficients(const Vector &b) {
coef().set_included_coefficients(b);
}
Vector GlmModel::included_coefficients() const {
return coef().included_coefficients();
}
double GlmModel::predict(const ConstVectorView &x) const {
return coef().predict(x);
}
double GlmModel::Beta(uint I) const { return coef().Beta(I); }
void GlmModel::drop(uint p) { coef().drop(p); }
uint GlmModel::xdim() const { return coef().nvars_possible(); }
//Modify the original Surface by extrapolating the specified perimeter.
//The extrapolation is C2 continuous if the order >=4.
//The extrapolation is done so that extrapolating length is "length"
//at the position of the parameter value "param" of the perimeter.
MGRSBRep& MGRSBRep::extend(
int perimeter, //perimeter number of the Surface.
// =0:v=min, =1:u=max, =2:v=max, =3:u=min.
double param, // parameter value of above perimeter.
double length, //chord length to extend at the parameter param of the perimeter.
double dk){ //Coefficient of how curvature should vary at
// extrapolation start point. When dk=0, curvature keeps same, i.e.
// dK/dS=0. When dk=1, curvature becomes zero at length extrapolated point,
// i.e. dK/dS=-K/length at extrapolation start point.
// (S=parameter of arc length, K=Curvature at start point)
// That is, when dk reaches to 1 from 0, curve changes to flat.
assert(sdim()<=3);
assert(perimeter>=0 && perimeter<4);
const size_t ncd=surface_bcoef().sdim();
int at_start=1;//starting perimeter
size_t nu, nv;
size_t order; size_t n,m; MGKnotVector* t;
if(perimeter==1 || perimeter==3){ // Extrapolate to u-direction
order=order_u();
n=bdim_u();
t=&(knot_vector_u());
if(perimeter==1)
at_start=0;//ending perimeter
m=nv=bdim_v();
}else{
// Extrapolate to v-direction
order=order_v();
n=bdim_v();
t=&(knot_vector_v());
if(perimeter==2)
at_start=0;//ending perimeter
m=nu=bdim_u();
}
//(nu,nv) are new surface B-Rep dimensions of u and v.
//(order,n,t) is line B-rep to extrapolate.
//m is the number of line B-reps to extrapolate.
MGSPointSeq surf;
MGRLBRep lbtemp;
MGKnotVector& t1=lbtemp.knot_vector();
MGBPointSeq& coeftemp=lbtemp.line_bcoef();
coeftemp.resize(n,ncd);
double tse;
if(at_start)
tse=t->param_s();
else
tse=t->param_e();
MGPosition uv=perimeter_uv(perimeter,param);//Surface parameter value of param.
size_t ndu=0,ndv=0;
if(perimeter==0 || perimeter==2) ndv=1;
else ndu=1;
double slen=length/(eval(uv,ndu,ndv)).len();
int nnew; double firstd_len,dlen;
for(size_t i=0; i<m; i++){
if(perimeter==0 || perimeter==2){
for(size_t j=0; j<n; j++)
for(size_t k=0; k<ncd; k++) coeftemp(j,k)=coef(i,j,k);
}else{
for(size_t j=0; j<n; j++)
for(size_t k=0; k<ncd; k++) coeftemp(j,k)=coef(j,i,k);
}
coeftemp.set_length(n);
//Compute first derivative length at the end of the extrapolating line.
t1=*t;
firstd_len=lbtemp.eval(tse,1).len();
dlen=firstd_len*slen;
//std::cout<<"before:"<<lbtemp<<std::endl;///////
lbtemp.extend(at_start,dlen,dk);
//std::cout<<"after:"<<lbtemp<<std::endl;///////
nnew=lbtemp.bdim();
if(perimeter==0 || perimeter==2){
if(i==0){
nv=nnew;
surf.resize(nu,nv,ncd);
}
for(int j=0; j<nnew; j++)
for(size_t k=0; k<ncd; k++) surf(i,j,k)=coeftemp(j,k);
}else{
if(i==0){
nu=nnew;
surf.resize(nu,nv,ncd);
}
for(int j=0; j<nnew; j++)
for(size_t k=0; k<ncd; k++) surf(j,i,k)=coeftemp(j,k);
}
}
*t=t1;
surf.set_length(nu,nv);
surface_bcoef()=surf;
update_mark();
return *this;
}
void
EigenExecutionerBase::chebyshev(Chebyshev_Parameters & chebyshev_parameters, unsigned int iter, const PostprocessorValue * solution_diff)
{
if (!solution_diff) mooseError("solution diff is required for Chebyshev acceleration");
if (chebyshev_parameters.lgac==0)
{
if (chebyshev_parameters.icho==0)
chebyshev_parameters.ratio = *solution_diff / chebyshev_parameters.flux_error_norm_old;
else
{
chebyshev_parameters.ratio = chebyshev_parameters.ratio_new;
chebyshev_parameters.icho = 0;
}
if (iter > chebyshev_parameters.finit &&
chebyshev_parameters.ratio>=0.4 &&
chebyshev_parameters.ratio<=1)
{
chebyshev_parameters.lgac = 1;
chebyshev_parameters.icheb = 1;
chebyshev_parameters.error_begin = *solution_diff;
chebyshev_parameters.iter_begin = iter;
double alp = 2/(2-chebyshev_parameters.ratio);
std::vector<double> coef(2);
coef[0] = alp;
coef[1] = 1-alp;
_eigen_sys.combineSystemSolution(EigenSystem::EIGEN, coef);
_problem.computeUserObjects(EXEC_LINEAR, UserObjectWarehouse::PRE_AUX);
_problem.computeAuxiliaryKernels(EXEC_LINEAR);
_problem.computeUserObjects(EXEC_LINEAR, UserObjectWarehouse::POST_AUX);
_eigenvalue = _source_integral;
}
}
else
{
chebyshev_parameters.icheb++;
double gamma = acosh(2/chebyshev_parameters.ratio-1);
double alp = 4/chebyshev_parameters.ratio*std::cosh((chebyshev_parameters.icheb-1)*gamma)
/std::cosh(chebyshev_parameters.icheb*gamma);
double beta = (1-chebyshev_parameters.ratio/2)*alp - 1;
/* if (iter<int(chebyshev_parameters.iter_begin+chebyshev_parameters.n_iter))
{
std::vector<double> coef(3);
coef[0] = alp;
coef[1] = 1-alp+beta;
coef[2] = -beta;
_eigen_sys.combineSystemSolution(NonlinearSystem::EIGEN, coef);
}
else
{*/
double gamma_new = (*solution_diff/chebyshev_parameters.error_begin)*
(std::cosh((chebyshev_parameters.icheb-1)*acosh(2/chebyshev_parameters.ratio-1)));
if (gamma_new<1.0) gamma_new = 1.0;
chebyshev_parameters.ratio_new = chebyshev_parameters.ratio/2*
(std::cosh(acosh(gamma_new) / (chebyshev_parameters.icheb-1))+1);
if (gamma_new>1.01)
{
chebyshev_parameters.lgac = 0;
// chebyshev_parameters.icheb = 0;
// if (chebyshev_parameters.icheb>30)
// {
if (chebyshev_parameters.icheb>0)
{
chebyshev_parameters.icho = 1;
chebyshev_parameters.finit = iter;
}
else
{
chebyshev_parameters.icho = 0;
chebyshev_parameters.finit = iter + chebyshev_parameters.fsmooth;
}
}
else
{
std::vector<double> coef(3);
coef[0] = alp;
coef[1] = 1-alp+beta;
coef[2] = -beta;
_eigen_sys.combineSystemSolution(EigenSystem::EIGEN, coef);
_problem.computeUserObjects(EXEC_LINEAR, UserObjectWarehouse::PRE_AUX);
_problem.computeAuxiliaryKernels(EXEC_LINEAR);
_problem.computeUserObjects(EXEC_LINEAR, UserObjectWarehouse::POST_AUX);
_eigenvalue = _source_integral;
}
// }
}
chebyshev_parameters.flux_error_norm_old = *solution_diff;
}
Polynom::Polynom(const string str)
{
int tmp, tmp1, tmp2;
coefficients.clear();
if (str.length() < 8)
throw(invalid_argument("incorrect number input. You can try help\n"));
for (auto i = 0; i < str.length(); i++)
if (!isdigit(str[i]) && str[i] != 'x' && str[i] != '^' && str[i] != '+' && str[i] != '-' && str[i] != '/' && str[i] != '(' && str[i] != ')')
throw(invalid_argument("incorrect number input. You can try help\n"));
tmp = 0;
if (str[0] == '-' || str[0] == '+')
tmp++;
do
{
tmp1 = str.find("(", tmp);
if (tmp1 != tmp)
throw(invalid_argument("incorrect number input. You can try help\n"));
tmp = ++tmp1;
while (tmp < str.size() && isdigit(str[tmp]))
tmp++;
if (tmp == tmp1)
throw(invalid_argument("incorrect number input. You can try help\n"));
if (tmp != -1)
{
tmp1 = str.find("/", tmp);
if (tmp1 == -1 || (tmp1 - tmp) != 0)
throw(invalid_argument("incorrect number input. You can try help\n"));
tmp = ++tmp1;
while (tmp < str.size() && isdigit(str[tmp]))
tmp++;
if (tmp == tmp1)
throw(invalid_argument("incorrect number input. You can try help\n"));
tmp1 = str.find(")", tmp);
if (tmp1 == -1 || (tmp1 - tmp) != 0)
throw(invalid_argument("incorrect number input. You can try help\n"));
tmp = ++tmp1;
tmp1 = str.find("x^", tmp);
if (tmp1 == -1 || (tmp1 - tmp) != 0)
throw(invalid_argument("incorrect number input. You can try help\n"));
tmp1 += 2;
while (tmp1 < str.size() && isdigit(str[tmp1]))
tmp1++;
if (tmp == tmp1)
throw(invalid_argument("incorrect number input. You can try help\n"));
if (tmp1 == str.size())
break;
tmp2 = tmp = tmp1;
tmp = str.find("+", tmp);
tmp1 = str.find("-", tmp1);
if (tmp == -1 && tmp1 == -1)
throw(invalid_argument("incorrect number input. You can try help\n"));
if (tmp == -1)
tmp = tmp1;
if (tmp1 < tmp)
tmp = tmp1;
if ((tmp - tmp2) != 0)
throw(invalid_argument("incorrect number input. You can try help\n"));
tmp++;
}
else
throw(invalid_argument("incorrect number input. You can try help\n"));
}
while (true);
MegaNatural lastCoef,b_lastCoef;
int sign;
MegaRational coef();
bool first = true;
int pos = 0;
do
{
if (first)
{
first = false;
if (str[0] == '-')
sign = -1;
else
sign = 1;
MegaInteger numerator(getNextNum(str, pos));
numerator = numerator*(MegaInteger)(-1);
MegaNatural denominator(getNextNum(str, pos));
coefficients.push_front(MegaRational(numerator, denominator));
lastCoef = getNextNum(str, pos);
continue;
}
tmp = str.find("+", pos);
tmp1 = str.find("-", pos);
if (tmp == -1 && tmp1 == -1)
break;
if (tmp != -1 && tmp1 != -1)
if (tmp < tmp1)
sign = 1;
else
sign = -1;
if (tmp == -1)
sign = -1;
if (tmp1 == -1)
sign = 1;
MegaInteger numerator(getNextNum(str, pos));
numerator = numerator*(MegaInteger) (-1);
MegaNatural denominator(getNextNum(str, pos));
coefficients.push_front(MegaRational(numerator, denominator));
b_lastCoef = lastCoef;
lastCoef = getNextNum(str, pos);
if (lastCoef >= b_lastCoef)
{
coefficients.clear();
coefficients.resize(0);
throw(invalid_argument("incorrect number input. You can try help\n"));
}
else
{
while ((b_lastCoef - (MegaNatural) 1) > lastCoef)
{
b_lastCoef = b_lastCoef - (MegaNatural) 1;
coefficients.push_front(MegaRational());
}
}
}
while (pos < str.size());
while (lastCoef>(MegaNatural)0)
{
lastCoef = lastCoef - (MegaNatural)1;
coefficients.push_front(MegaRational());
}
}
// reports 0 for excluded positions
const Vector &GlmModel::Beta() const { return coef().Beta(); }
void GlmModel::set_Beta(const Vector &B) { coef().set_Beta(B); }
void GlmModel::flip(uint p) { coef().flip(p); }
const Selector &GlmModel::inc() const { return coef().inc(); }
bool GlmModel::inc(uint p) const { return coef().inc(p); }
void GlmModel::add(uint p) { coef().add(p); }
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;
}