int Lu::objpose(MatrixXf P, MatrixXf Qp)
{
int n = P.cols();
int i;
// move the origin to the center of P
// std::cout<<"P\n"<<P<<std::endl;
Vector3f pbar=Vector3f::Zero();
// std::cout<<"pbar\n"<<pbar<<std::endl;
for (i = 0; i<n;i++){
// std::cout<<"Pcol\n"<<P.col(i)<<std::endl;
pbar=pbar+P.col(i);
}
//std::cout<<"pbar\n"<<pbar;
pbar= pbar/n;
// std::cout<<"move the origin to the center of P"<<std::endl;
//std::cout<<"P\n"<<P;
//std::cout<<"pbar\n"<<pbar<<std::endl;
for (i = 0; i<n;i++){
P.col(i) = P.col(i)-pbar;
}
//std::cout<<"P\n"<<P<<std::endl;
MatrixXf Q;
Q=MatrixXf::Zero(3,n);
for (i = 0 ;i<n;i++){
Q(0,i)=Qp(0,i);
Q(1,i)=Qp(1,i);
Q(2,i)=1;
}
//std::cout<<"Q\n"<<Q<<std::endl;
//create matrix F with 0's
//Matrix3f maux;
//for (i=0;i<n;i++ ){
// F.push_back(Matrix3f::Zero(3,3));
//}
RowVector3f V;
V= RowVector3f::Zero();
std::vector<Matrix3f > F;
Matrix3f F_aux;
for (i = 0 ;i<n;i++){
V[0] = Q(0,i)/Q(2,i);
V[1] = Q(1,i)/Q(2,i);
V[2] = Q(2,i)/Q(2,i);
//std::cout<<"V\n"<<V<<std::endl;
Matrix3f F_aux;
F_aux=(V.transpose()*V)/V.dot(V);
//std::cout<<"F_aux\n"<<F_aux<<std::endl;
// F_aux = (V*V.transpose())/(V.transpose()*V);
F.push_back(F_aux);
}
//compute the matrix factor required to compute t
Matrix3f F_sum=Matrix3f::Zero(3,3);
for (i = 0 ;i< n;i++){
F_sum=F_sum + F[i];
}
//std::cout<<"F_sum\n"<<F_sum<<std::endl;
Matrix3f tFactor = (Matrix3f::Identity()-F_sum/n);
tFactor=tFactor.inverse()/n;
//std::cout<<"tfactor\n"<<tFactor<<std::endl;
MatrixXf Ri_;
MatrixXf ti_;
MatrixXf Qi_;
double old_err;
// compute initial pose estimate
abskernel(P, Q, F, tFactor);
//Lu::abskernel(MatrixXf P, MatrixXf Q, std::vector<Matrix3f > F, MatrixXf G)
Ri_=GetRi();
ti_=Getti();
Qi_=GetQout();
int it = 1;
old_err=Geterr();
/*std::cout<<"Ri_\n"<<Ri_<<std::endl;
std::cout<<"ti_\n"<<ti_<<std::endl;
std::cout<<"Qi_\n"<<Qi_<<std::endl;
std::cout<<"old_err\n"<<old_err<<std::endl;*/
//std::cout<<"old_err\n"<<old_err<<std::endl;
// compute next pose estimate
abskernel(P, Qi_, F, tFactor);
Ri_=GetRi();
ti_=Getti();
Qi_=GetQout();
double new_err=Geterr();
it = it + 1;
/*std::cout<<"Ri_2\n"<<Ri_<<std::endl;
std::cout<<"ti_2\n"<<ti_<<std::endl;
std::cout<<"Qi_2\n"<<Qi_<<std::endl;
std::cout<<"new_err\n"<<new_err<<std::endl;*/
//std::cout<<"new_err\n"<<new_err<<std::endl;
double val=(old_err-new_err)/old_err;
val=sqrt(val*val);
/* std::cout<<"val\n"<<val<<std::endl;
std::cout<<"TOL\n"<<TOL<<std::endl;
std::cout<<"EPSILON\n"<<EPSILON<<std::endl;
std::cout<<"MAX_ITER\n"<<MAX_ITER<<std::endl;*/
while ((val > TOL) && (new_err > EPSILON) && (it<MAX_ITER)){
old_err = new_err;
//std::cout<<"old_err\n"<<old_err<<std::endl;
// compute the optimal estimate of R
abskernel(P, Qi_, F, tFactor);
Ri_=GetRi();
ti_=Getti();
Qi_=GetQout();
new_err=Geterr();
it = it + 1;
}
R = Ri;
t = ti_;
obj_err = sqrt(new_err/n);
//double img_err = sqrt(img_err/n);
t = t - Ri*pbar;
return 0;
}
void objpose(mat33_t *R, vec3_t *t, int *it, real_t *obj_err, real_t *img_err,
bool calc_img_err, const vec3_t *_P, const vec3_t *Qp, const options_t options, const int n)
{
int i, j;
//vec3_array P(_P.begin(),_P.end());
vec3_t P[n];
memcpy(&*P, _P, n*sizeof(vec3_t));
//const int n = (unsigned int) P.size();
vec3_t pbar;
vec3_array_sum(&pbar, &*P, n);
vec3_div(&pbar, (real_t)(n));
vec3_array_sub(&*P, &pbar, n);
//vec3_array Q(Qp.begin(),Qp.end());
vec3_t Q[n];
memcpy(&*Q, Qp, n*sizeof(vec3_t));
vec3_t ones;
ones.v[0] = 1;
ones.v[1] = 1;
ones.v[2] = 1;
const bool mask_z[3] = {0,0,1};
vec3_array_set(&*Q, &ones, mask_z, n);
//mat33_array F;
//F.resize(n);
mat33_t F[n];
vec3_t V;
for(i=0; i<n; i++)
{
V.v[0] = Q[i].v[0] / Q[i].v[2];
V.v[1] = Q[i].v[1] / Q[i].v[2];
V.v[2] = 1.0;
mat33_t _m;
vec3_mul_vec3trans(&_m, &V, &V);
mat33_div(&_m, vec3trans_mul_vec3(&V,&V));
F[i] = _m;
}
mat33_t tFactor;
mat33_t _m1,_m2,_m3;
mat33_eye(&_m1);
mat33_array_sum(&_m2, &*F, n);
mat33_div(&_m2, (real_t)(n));
mat33_sub_mat2(&_m3, &_m1, &_m2);
mat33_inv(&tFactor, &_m3);
mat33_div(&tFactor, (real_t)(n));
*it = 0;
int initR_approximate = mat33_all_zeros(&options.initR);
mat33_t Ri;
vec3_t ti;
//vec3_array Qi;
//Qi.resize(n);
vec3_t Qi[n];
real_t old_err = 0.0, new_err = 0.0;
// ----------------------------------------------------------------------------------------
if(initR_approximate == 0)
{
mat33_copy(&Ri, &options.initR);
vec3_t _sum;
vec3_t _v1, _v2;
mat33_t _m1,_m2;
vec3_clear(&_sum);
for(j=0; j<n; j++)
{
mat33_eye(&_m1);
mat33_sub_mat2(&_m2, &F[j], &_m1);
vec3_mult_mat(&_v1, &Ri, &P[j]);
vec3_mult_mat(&_v2, &_m2, &_v1);
vec3_add_vec(&_sum, &_v2);
}
vec3_mult_mat(&ti,&tFactor,&_sum);
xform(&*Qi, &*P, &Ri, &ti, n);
old_err = 0;
vec3_t _v;
for(j=0; j<n; j++)
{
mat33_eye(&_m1);
mat33_sub_mat2(&_m2, &F[j], &_m1);
vec3_mult_mat(&_v, &_m2, &Qi[j]);
old_err += vec3_dot(&_v, &_v);
}
// ----------------------------------------------------------------------------------------
}
else
{
abskernel(&Ri, &ti, &*Qi, &old_err, &*P, &*Q, &*F, &tFactor, n);
*it = 1;
}
// ----------------------------------------------------------------------------------------
abskernel(&Ri, &ti, &*Qi, &new_err, &*P, &*Qi, &*F, &tFactor, n);
*it = *it + 1;
while((_abs((old_err-new_err)/old_err) > options.tol) && (new_err > options.epsilon) &&
(options.max_iter == 0 || *it < options.max_iter))
{
old_err = new_err;
abskernel(&Ri, &ti, &*Qi, &new_err, &*P, &*Qi, &*F, &tFactor, n);
*it = *it + 1;
}
mat33_copy(R, &Ri);
vec3_copy(t, &ti);
*obj_err = _sqrt(new_err/(real_t)(n));
if(calc_img_err == 1)
{
//vec3_array Qproj;
//Qproj.resize(n);
vec3_t Qproj[n];
xformproj(&*Qproj, &*P, &Ri, &ti, n);
*img_err = 0;
vec3_t _v;
for(j=0; j<n; j++)
{
vec3_sub_vec2(&_v, &Qproj[j], &Qp[j]);
*img_err += vec3_dot(&_v, &_v);
}
*img_err = _sqrt(*img_err/(real_t)(n));
}
if(t->v[2] < 0)
{
mat33_mult(R, -1.0);
vec3_mult(t, -1.0);
}
vec3_t _ts;
vec3_mult_mat(&_ts, &Ri, &pbar);
vec3_sub_vec(t, &_ts);
}
