AtomsPDB & AtomsPDB::operator=(const AtomsPDB & y){ try{ if(nr != y.nr) throw "Cannot copy AtomsPDB of different size "; } catch(const char * s){ std::cout << s << std::endl; exit(1); } for(int i=0;i<nr;i++) { x[i][0]=y.x[i][0]; x[i][1]=y.x[i][1]; x[i][2]=y.x[i][2]; } Mt(y.Mt); if(!MyId) MyId=new AtomIndex; else { delete MyId; MyId=new AtomIndex; } *MyId=*(y.MyId); init_t_atoms(&useatoms,y.useatoms.nr,FALSE); delete [] useatoms.resinfo; useatoms.resinfo = y.useatoms.resinfo; for(int i=0;i<y.useatoms.nr;i++) { useatoms.atomname[i]=y.useatoms.atomname[i]; useatoms.atom[i]=y.useatoms.atom[i]; useatoms.nres=max(useatoms.nres,y.useatoms.atom[i].resind+1); } useatoms.nr=y.useatoms.nr; return *this; }
void AtomsPDB::setCoord(const Metric & Mt_in, const rvec * x0){ Mt(Mt_in); for(int i=0;i<MyId->getN();i++) for(int j=0;j<DIM;j++){ x[i][j]=x0[(*MyId)[i]][j]; } }
//Умножение матриц Matrix matmul(const Matrix &A, const Matrix &B) throw (int) { unsigned int aWidth, aHeight, bWidth, bHeight; aWidth = A.getWidth(); aHeight = A.getHeight(); bWidth = B.getWidth(); bHeight = B.getHeight(); if (aWidth != bHeight) throw Matrix::SIZE_EXCEPTION; Matrix Mt(aHeight, bWidth, A.eps); unsigned int i, j, k; for (i = 0; i < aHeight; ++i) { for (j = 0; j < bWidth; ++j) { Mt.M[i][j] = 0; for (k = 0; k < aWidth; ++k) { Mt.M[i][j] += A.M[i][k] * B.M[k][j]; } } } return Mt; }
/* void invert(valarray<double>& Mi,const valarray<double>& M){ int Dim = sqrt(double(M.size())); vector<int> indx(Dim); valarray<double> Mt = M; valarray<double> vv(Dim); //// LU decomposition for(int i=0; i<Dim; ++i){ double big = 0.0; for(int j=0; j<Dim; ++j) big = max(big,fabs(Mt[i*Dim+j])); vv[i] = 1.0/big; } for(int j=0; j<Dim; ++j){ for(int i=0; i<j; ++i){ double sum = Mt[i*Dim+j]; for(int k=0; k<i; ++k) sum -= Mt[i*Dim+k]*Mt[k*Dim+j]; Mt[i*Dim+j] = sum; } double big=0.0; int imax; for(int i=j; i<Dim; ++i){ imax = j; double sum = Mt[i*Dim+j]; for(int k=0; k<j; ++k) sum -= Mt[i*Dim+k]*Mt[k*Dim+j]; Mt[i*Dim+j] = sum; if(double dum= vv[i]*fabs(sum) >= big){ big = dum; imax = i; } } if(j!=imax){ for(int k=0; k<Dim; ++k) swap(Mt[imax*Dim+k],Mt[j*Dim+k]); vv[imax] = vv[j]; } indx[j]= imax; if(j !=Dim-1) for(int i=j+1; i<Dim; ++i) Mt[i*Dim+j] /= Mt[j*Dim+j]; } /////// for(int k=0; k<Dim; ++k){ for(int l=0; l<Dim; ++l) CCIO::cout<<" LU["<<k<<","<<l<<"]="<<Mt[k*Dim+l]; CCIO::cout<<"\n"; } //// forward/backward subtractions for(int j=0; j<Dim; ++j){ vector<double> col(Dim,0.0); col[j] = 1.0; int ii = 0; for(int i=0; i<Dim; ++i){ double sum = col[indx[i]]; col[indx[i]] = col[i]; if(ii) for(int k=ii; k<i; ++k) sum -= Mt[i*Dim+k]*col[k]; else if(sum) ii = i; col[i] = sum; } for(int i=Dim-1; i>=0; --i){ double sum = col[i]; for(int k=i+1; k<Dim; ++k) sum -= Mt[i*Dim+k]*col[k]; col[i] = sum/Mt[i*Dim+i]; } for(int i=0; i<Dim; ++i) Mi[i*Dim+j] = col[i]; } } */ void invert(valarray<double>& Mi,const valarray<double>& M){ int Dim = sqrt(double(M.size())); valarray<double> Mt(M); gsl_matrix_view m = gsl_matrix_view_array(&(Mt[0]),Dim,Dim); gsl_matrix_view mi = gsl_matrix_view_array(&(Mi[0]),Dim,Dim); gsl_permutation* p = gsl_permutation_alloc(Dim); int sgn; gsl_linalg_LU_decomp(&m.matrix,p,&sgn); gsl_linalg_LU_invert(&m.matrix,p,&mi.matrix); }
int main() { std::mt19937 Mt; Mt.seed(1234); for (u32 StateIndex = 0; StateIndex < 627; ++StateIndex) { printf("%x\n", Mt()); } }
//Транспонирование Matrix Matrix::transp() const { Matrix Mt(height, width, eps); unsigned int i, j; for (i = 0; i < height; ++i) { for (j = 0; j < width; ++j) Mt.M[i][j] = M[j][i]; } return Mt; }
void OmniRobot::init() { period = 200; xw = 75.0; //mm yw = 75.0; //mm Dw = 50.0; //mm vector<float> u1 (2); u1(0) = c1; u1(1) = c1; vector<float> u2 (2); u2(0) = c1; u2(1) = -c1; vector<float> u3 (2); u3(0) = c1; u3(1) = c1; vector<float> u4 (2); u4(0) = c1; u4(1) = -c1; vector<float> n1 (2); n1(0) = c1; n1(1) = -c1; vector<float> n2 (2); n2(0) = -c1; n2(1) = -c1; vector<float> n3 (2); n3(0) = c1; n3(1) = -c1; vector<float> n4 (2); n4(0) = -c1; n4(1) = -c1; vector<float> b1 (2); b1(0) = xw; b1(1) = yw; vector<float> b2 (2); b2(0) = xw; b2(1) = -yw; vector<float> b3 (2); b3(0) = -xw; b3(1) = -yw; vector<float> b4 (2); b4(0) = -xw; b4(1) = yw; Mt(0,0) = n1(0); Mt(0,1) = n1(1); Mt(0,2) = b1(0)*u1(0) + b1(1)*u1(1); Mt(1,0) = n2(0); Mt(1,1) = n2(1); Mt(1,2) = b2(0)*u2(0) + b2(1)*u2(1); Mt(2,0) = n3(0); Mt(2,1) = n3(1); Mt(2,2) = b3(0)*u3(0) + b3(1)*u3(1); Mt(3,0) = n4(0); Mt(3,1) = n3(1); Mt(3,2) = b4(0)*u4(0) + b4(1)*u4(1); Mt = -1 * Mt; cmd(0) = 0.0; cmd(1) = 0.0; cmd(2) = 0.0; pwm(0) = 0.0; pwm(1) = 0.0; pwm(2) = 0.0; pwm(3) = 0.0; omniState = INIT_MODE; movementMode = ROTATE_MODE; power = 20; pplus = 1; }
double Likelihood<V, M>::lnValue(const QUESO::GslVector & paramValues) const { double resultValue = 0.; m_env.subComm().Barrier(); //env.syncPrintDebugMsg("Entering likelihoodRoutine()",1,env.fullComm()); // Compute likelihood for scenario 1 double betaTest = m_beta1; if (betaTest) { double A = paramValues[0]; double E = paramValues[1]; double beta = m_beta1; double variance = m_variance1; const std::vector<double>& Te = m_Te1; const std::vector<double>& Me = m_Me1; std::vector<double> Mt(Me.size(),0.); double params[]={A,E,beta}; // integration const gsl_odeiv_step_type *T = gsl_odeiv_step_rkf45; //rkf45; //gear1; gsl_odeiv_step *s = gsl_odeiv_step_alloc(T,1); gsl_odeiv_control *c = gsl_odeiv_control_y_new(1e-6,0.0); gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc(1); gsl_odeiv_system sys = {func, NULL, 1, (void *)params}; double t = 0.1, t_final = 1900.; double h = 1e-3; double Mass[1]; Mass[0]=1.; unsigned int i = 0; double t_old = 0.; double M_old[1]; M_old[0]=1.; double misfit=0.; //unsigned int loopSize = 0; while ((t < t_final) && (i < Me.size())) { int status = gsl_odeiv_evolve_apply(e, c, s, &sys, &t, t_final, &h, Mass); UQ_FATAL_TEST_MACRO((status != GSL_SUCCESS), paramValues.env().fullRank(), "likelihoodRoutine()", "gsl_odeiv_evolve_apply() failed"); //printf("t = %6.1lf, mass = %10.4lf\n",t,Mass[0]); //loopSize++; while ( (i < Me.size()) && (t_old <= Te[i]) && (Te[i] <= t) ) { Mt[i] = (Te[i]-t_old)*(Mass[0]-M_old[0])/(t-t_old) + M_old[0]; misfit += (Me[i]-Mt[i])*(Me[i]-Mt[i]); //printf("%i %lf %lf %lf %lf\n",i,Te[i],Me[i],Mt[i],misfit); i++; } t_old=t; M_old[0]=Mass[0]; } resultValue += misfit/variance; //printf("loopSize = %d\n",loopSize); if ((paramValues.env().displayVerbosity() >= 10) && (paramValues.env().fullRank() == 0)) { printf("In likelihoodRoutine(), A = %g, E = %g, beta = %.3lf: misfit = %lf, likelihood = %lf.\n",A,E,beta,misfit,resultValue); } gsl_odeiv_evolve_free (e); gsl_odeiv_control_free(c); gsl_odeiv_step_free (s); } // Compute likelihood for scenario 2 betaTest = m_beta2; if (betaTest > 0.) { double A = paramValues[0]; double E = paramValues[1]; double beta = m_beta2; double variance = m_variance2; const std::vector<double>& Te = m_Te2; const std::vector<double>& Me = m_Me2; std::vector<double> Mt(Me.size(),0.); double params[]={A,E,beta}; // integration const gsl_odeiv_step_type *T = gsl_odeiv_step_rkf45; //rkf45; //gear1; gsl_odeiv_step *s = gsl_odeiv_step_alloc(T,1); gsl_odeiv_control *c = gsl_odeiv_control_y_new(1e-6,0.0); gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc(1); gsl_odeiv_system sys = {func, NULL, 1, (void *)params}; double t = 0.1, t_final = 1900.; double h = 1e-3; double Mass[1]; Mass[0]=1.; unsigned int i = 0; double t_old = 0.; double M_old[1]; M_old[0]=1.; double misfit=0.; //unsigned int loopSize = 0; while ((t < t_final) && (i < Me.size())) { int status = gsl_odeiv_evolve_apply(e, c, s, &sys, &t, t_final, &h, Mass); UQ_FATAL_TEST_MACRO((status != GSL_SUCCESS), paramValues.env().fullRank(), "likelihoodRoutine()", "gsl_odeiv_evolve_apply() failed"); //printf("t = %6.1lf, mass = %10.4lf\n",t,Mass[0]); //loopSize++; while ( (i < Me.size()) && (t_old <= Te[i]) && (Te[i] <= t) ) { Mt[i] = (Te[i]-t_old)*(Mass[0]-M_old[0])/(t-t_old) + M_old[0]; misfit += (Me[i]-Mt[i])*(Me[i]-Mt[i]); //printf("%i %lf %lf %lf %lf\n",i,Te[i],Me[i],Mt[i],misfit); i++; } t_old=t; M_old[0]=Mass[0]; } resultValue += misfit/variance; //printf("loopSize = %d\n",loopSize); if ((paramValues.env().displayVerbosity() >= 10) && (paramValues.env().fullRank() == 0)) { printf("In likelihoodRoutine(), A = %g, E = %g, beta = %.3lf: misfit = %lf, likelihood = %lf.\n",A,E,beta,misfit,resultValue); } gsl_odeiv_evolve_free (e); gsl_odeiv_control_free(c); gsl_odeiv_step_free (s); } // Compute likelihood for scenario 3 betaTest = m_beta3; if (betaTest > 0.) { double A = paramValues[0]; double E = paramValues[1]; double beta = m_beta3; double variance = m_variance3; const std::vector<double>& Te = m_Te3; const std::vector<double>& Me = m_Me3; std::vector<double> Mt(Me.size(),0.); double params[]={A,E,beta}; // integration const gsl_odeiv_step_type *T = gsl_odeiv_step_rkf45; //rkf45; //gear1; gsl_odeiv_step *s = gsl_odeiv_step_alloc(T,1); gsl_odeiv_control *c = gsl_odeiv_control_y_new(1e-6,0.0); gsl_odeiv_evolve *e = gsl_odeiv_evolve_alloc(1); gsl_odeiv_system sys = {func, NULL, 1, (void *)params}; double t = 0.1, t_final = 1900.; double h = 1e-3; double Mass[1]; Mass[0]=1.; unsigned int i = 0; double t_old = 0.; double M_old[1]; M_old[0]=1.; double misfit=0.; //unsigned int loopSize = 0; while ((t < t_final) && (i < Me.size())) { int status = gsl_odeiv_evolve_apply(e, c, s, &sys, &t, t_final, &h, Mass); UQ_FATAL_TEST_MACRO((status != GSL_SUCCESS), paramValues.env().fullRank(), "likelihoodRoutine()", "gsl_odeiv_evolve_apply() failed"); //printf("t = %6.1lf, mass = %10.4lf\n",t,Mass[0]); //loopSize++; while ( (i < Me.size()) && (t_old <= Te[i]) && (Te[i] <= t) ) { Mt[i] = (Te[i]-t_old)*(Mass[0]-M_old[0])/(t-t_old) + M_old[0]; misfit += (Me[i]-Mt[i])*(Me[i]-Mt[i]); //printf("%i %lf %lf %lf %lf\n",i,Te[i],Me[i],Mt[i],misfit); i++; } t_old=t; M_old[0]=Mass[0]; } resultValue += misfit/variance; //printf("loopSize = %d\n",loopSize); if ((paramValues.env().displayVerbosity() >= 10) && (paramValues.env().fullRank() == 0)) { printf("In likelihoodRoutine(), A = %g, E = %g, beta = %.3lf: misfit = %lf, likelihood = %lf.\n",A,E,beta,misfit,resultValue); } gsl_odeiv_evolve_free (e); gsl_odeiv_control_free(c); gsl_odeiv_step_free (s); } m_env.subComm().Barrier(); //env.syncPrintDebugMsg("Leaving likelihoodRoutine()",1,env.fullComm()); return -.5*resultValue; }
void Calib::computeProjectionMatrix() { std::cout << "1111111111***********************************************************************************\n"; // for(int i = 0;i < imagePoints.size();i++) // { // std::cout<<" imagePoints[i]= "<< imagePoints[i]<<std::endl; // std::cout<<" objectPoints[i]= "<< objectPoints[i]<<std::endl; // } if (objectPoints.size() != imagePoints.size()) return; int n = objectPoints.size(); Eigen::Vector2d avg2; Eigen::Vector3d avg3; for (unsigned int i = 0; i < n; i++) { avg2 += imagePoints[i]; avg3 += objectPoints[i]; std::cout << i << " " << objectPoints[i](0) << " " << objectPoints[i](1) << " " << objectPoints[i](2) << std::endl; } avg2 = avg2 / n; avg3 = avg3 / n; std::cout << "avg2 = " << avg2 << std::endl; std::cout << "avg3 = " << avg3 << std::endl; /* ******************************************************************************* * Data normalization * Translates and normalises a set of 2D homogeneous points so that their centroid is at the origin and their mean distance from the origin is sqrt(2). */ float meandist2 = 0; float meandist3 = 0; imagePoints_t.resize(n); objectPoints_t.resize(n); for (unsigned int i = 0; i < n; i++) { imagePoints_t[i] = imagePoints[i] - avg2; objectPoints_t[i] = objectPoints[i] - avg3; // std::cout << "1 imagePoints_t[i] = " << imagePoints_t[i] << std::endl; // std::cout << "1 objectPoints_t[i] = " << objectPoints_t[i] << std::endl; meandist2 += sqrt(imagePoints_t[i](0) * imagePoints_t[i](0) + imagePoints_t[i](1) * imagePoints_t[i](1)); meandist3 += sqrt(objectPoints_t[i](0) * objectPoints_t[i](0) + objectPoints_t[i](1) * objectPoints_t[i](1) + objectPoints_t[i](2) * objectPoints_t[i](2)); } meandist2 /= n; meandist3 /= n; std::cout << "meandist2 = " << meandist2 << std::endl; std::cout << "meandist3 = " << meandist3 << std::endl; float scale2 = sqrt(2) / meandist2; float scale3 = sqrt(3) / meandist3; std::cout << "2222222222222***********************************************************************************\n"; for (unsigned int i = 0; i < n; i++) { imagePoints_t[i] = scale2 * imagePoints_t[i]; objectPoints_t[i] = scale3 * objectPoints_t[i]; // std::cout << "imagePoints_t[i] = " << imagePoints_t[i] << std::endl; // std::cout << "objectPoints_t[i] = " << objectPoints_t[i] << std::endl; } // std::cout<<avg3<<std::endl; /* ******************************************************************************* * Similarity transformation T1 to normalize the image points, * and a second similarity transformation T2 to normalize the space points. * Page 181 in Multiple_View_Geometry_in_Computer_Vision__2nd_Edition */ Eigen::Matrix3d T1; T1 << scale2, 0, -scale2 * avg2(0), 0, scale2, -scale2 * avg2(1), 0, 0, 1; Eigen::MatrixXd T2(4, 4); T2 << scale3, 0, 0, -scale3 * avg3(0), 0, scale3, 0, -scale3 * avg3(1), 0, 0, scale3, -scale3 * avg3(2), 0, 0, 0, 1; // use Eigen Eigen::MatrixXd A(2 * n, 12); A.setZero(2 * n, 12); for (int i = 0; i < n; i++) { A(2 * i, 0) = objectPoints_t[i](0); A(2 * i, 1) = objectPoints_t[i](1); A(2 * i, 2) = objectPoints_t[i](2); A(2 * i, 3) = 1; A(2 * i, 4) = 0; A(2 * i, 5) = 0; A(2 * i, 6) = 0; A(2 * i, 7) = 0; A(2 * i, 8) = -imagePoints_t[i](0) * objectPoints_t[i](0); A(2 * i, 9) = -imagePoints_t[i](0) * objectPoints_t[i](1); A(2 * i, 10) = -imagePoints_t[i](0) * objectPoints_t[i](2); A(2 * i, 11) = -imagePoints_t[i](0) * 1; A(2 * i + 1, 0) = 0; A(2 * i + 1, 1) = 0; A(2 * i + 1, 2) = 0; A(2 * i + 1, 3) = 0; A(2 * i + 1, 4) = 1 * objectPoints_t[i](0); A(2 * i + 1, 5) = 1 * objectPoints_t[i](1); A(2 * i + 1, 6) = 1 * objectPoints_t[i](2); A(2 * i + 1, 7) = 1; A(2 * i + 1, 8) = -imagePoints_t[i](1) * objectPoints_t[i](0); A(2 * i + 1, 9) = -imagePoints_t[i](1) * objectPoints_t[i](1); A(2 * i + 1, 10) = -imagePoints_t[i](1) * objectPoints_t[i](2); A(2 * i + 1, 11) = -imagePoints_t[i](1) * 1; } Eigen::JacobiSVD<Eigen::MatrixXd> svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); const Eigen::VectorXd & v1 = svd.matrixV().col(11); this->Pt << v1(0), v1(1), v1(2), v1(3), v1(4), v1(5), v1(6), v1(7), v1(8), v1(9), v1(10), v1(11); // std::cout<<"A= \n"<< A<<std::endl; // std::cout<<Pt<<std::endl; P = T1.inverse() * Pt * T2; //Decompose the projection matrix cv::Mat Pr(3, 4, cv::DataType<float>::type); cv::Mat Mt(3, 3, cv::DataType<float>::type); cv::Mat Rt(3, 3, cv::DataType<float>::type); cv::Mat Tt(4, 1, cv::DataType<float>::type); for (unsigned i = 0; i < 3; i++) { for (unsigned int j = 0; j < 4; j++) { Pr.at<float> (i, j) = P(i, j); } } cv::decomposeProjectionMatrix(Pr, Mt, Rt, Tt); //scale: Mt(2,2) should = 1; so update the projection matrix and decomposition again float k = (1 / (Mt.at<float> (2, 2))); /* **************************************************************************************** * Upate the projection matrix * Decomposition again to get new intrinsic matrix and rotation matrix */ this->P = k * P; cv::Mat Pro(3, 4, cv::DataType<float>::type); cv::Mat Mc(3, 3, cv::DataType<float>::type); // intrinsic parameter matrix cv::Mat Rc(3, 3, cv::DataType<float>::type); // rotation matrix cv::Mat Tc(4, 1, cv::DataType<float>::type); // translation vector for (unsigned i = 0; i < 3; i++) { for (unsigned int j = 0; j < 4; j++) { Pro.at<float> (i, j) = P(i, j); } } cv::decomposeProjectionMatrix(Pro, Mc, Rc, Tc); // Change from OpenCV varibles to Eigen for (unsigned i = 0; i < 3; i++) { for (unsigned int j = 0; j < 3; j++) { M(i, j) = Mc.at<float> (i, j) ; } } /* **************************************************************************************** * Compute te rotation matrix R and translation vector T */ P_temp = M.inverse() * P; this->R = this->P_temp.block(0,0,3,3); this->T = this->P_temp.block(0,3,3,1); std::cout << "+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++\n"; // std::cout << "T1 =\n " << T2 << std::endl; // std::cout << "T2 =\n " << T1 << std::endl; // std::cout << "A =\n " << A << std::endl; // std::cout << "svd.matrixV() =\n " << svd.matrixV() << std::endl; // std::cout << "Pt =\n " << Pt << std::endl; std::cout << "P =\n " << P << std::endl; std::cout << "M =\n " << M << std::endl; std::cout << "R =\n " << R << std::endl; std::cout << "Rc =\n " << Rc << std::endl; std::cout << "T =\n " << T << std::endl; std::cout << "Mt(2,2) = " << Mt.at<float> (2, 2) << std::endl; }