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;
}
