void Lu::compute(){
int n=x3d.cols();
//std::cout<<"No. points "<<n<<std::endl;
int val= objpose(x3d.block(0,0,3,n), x2dn.block(0,0,2,n));
//Estimate 3d position of the points in camera coordinates
MatrixXf xcam=MatrixXf::Zero(n,3); //recovered 3d positions optional
MatrixXf x3daux=x3d.block(0,0,3,n);
Vector3f xw;
Xcam=MatrixXf::Zero(3,n);
for (int i=0; i<n;i++){
xw=x3daux.col(i);
Xcam.col(i)=K*(R*xw+t);
Xcam.col(i)=Xcam.col(i)/Xcam(2,i);
//std::cout<<"xcam \n"<<Xcam<<std::endl;
}
}
void arGetInitRot2_sub2(real_t &err, mat33_t &R, vec3_t &t, const vec3_array &_model, const vec3_array &_iprts, const options_t _options)
{
mat33_t Rlu_;
vec3_t tlu_;
unsigned int it1_;
real_t obj_err1_;
real_t img_err1_;
vec3_array model(_model.begin(),_model.end());
vec3_array iprts(_iprts.begin(),_iprts.end());
options_t options;
memcpy(&options,&_options,sizeof(options_t));
mat33_clear(Rlu_);
vec3_clear(tlu_);
it1_ = 0;
obj_err1_ = 0;
img_err1_ = 0;
objpose(R, t, it1_, obj_err1_, img_err1_, false, model, iprts, options);
/*pose_vec sol;
sol.clear();
get2ndPose_Exact(sol,iprts,model,Rlu_,tlu_,0);
int min_err_idx = (-1);
real_t min_err = MAX_FLOAT;
for(unsigned int i=0; i<sol.size(); i++)
{
mat33_copy(options.initR,sol[i].R);
objpose(Rlu_, tlu_, it1_, obj_err1_, img_err1_, true, model, iprts, options);
mat33_copy(sol[i].PoseLu_R,Rlu_);
vec3_copy(sol[i].PoseLu_t,tlu_);
sol[i].obj_err = obj_err1_;
if(sol[i].obj_err < min_err)
{
min_err = sol[i].obj_err;
min_err_idx = i;
}
}
if(min_err_idx >= 0)
{
mat33_copy(R,sol[min_err_idx].PoseLu_R);
vec3_copy(t,sol[min_err_idx].PoseLu_t);
err = sol[min_err_idx].obj_err;
}
else
{
mat33_clear(R);
vec3_clear(t);
err = MAX_FLOAT;
}*/
}
// =====================================================================================
// function returns two solutions to the pose problem (1st has the lower Error !!!!!!
// ====================================================================================
void robust_pose(real_t *err, mat33_t *R , vec3_t *t,
real_t *errb, mat33_t *Rb, vec3_t *tb,
const vec3_t *_model, const vec3_t *_iprts, const options_t _options, const int n)
{
mat33_t Rlu_;
vec3_t tlu_;
int it1_, i;
real_t obj_err1_;
real_t img_err1_;
vec3_t model[n];
vec3_t iprts[n];
memcpy(&*model, _model, n * sizeof(vec3_t));
memcpy(&*iprts, _iprts, n * sizeof(vec3_t));
options_t options;
memcpy(&options, &_options, sizeof(options_t));
mat33_clear(&Rlu_);
vec3_clear(&tlu_);
it1_ = 0;
obj_err1_ = 0;
img_err1_ = 0;
objpose(&Rlu_, &tlu_, &it1_, &obj_err1_, &img_err1_, CALC_IMAGE_ERROR, &*model, &*iprts, options, n);
pose_t sol[10];
int sol_n = 0;
get2ndPose_Exact(&*sol, &*iprts, &*model, Rlu_, tlu_, 0, n, &sol_n);
// refine all poses
for(i=0; i<sol_n; i++)
{
mat33_copy(&options.initR, &sol[i].R);
objpose(&Rlu_, &tlu_, &it1_, &obj_err1_, &img_err1_, CALC_IMAGE_ERROR, &*model, &*iprts, options, n);
mat33_copy(&sol[i].PoseLu_R, &Rlu_);
vec3_copy(&sol[i].PoseLu_t, &tlu_);
sol[i].obj_err = obj_err1_;
}
// from all solutions get the best
int min_err_idx = (-1);
real_t min_err = MAX_FLOAT;
for(i=0; i<sol_n; i++)
if(sol[i].obj_err < min_err)
{
min_err = sol[i].obj_err;
min_err_idx = i;
}
// Copy the best solution !!!
if(min_err_idx >= 0)
{
mat33_copy(R, &sol[min_err_idx].PoseLu_R);
vec3_copy(t, &sol[min_err_idx].PoseLu_t);
*err = sol[min_err_idx].obj_err;
}
else
{
mat33_clear(R);
vec3_clear(t);
*err = MAX_FLOAT;
}
// from all solutions get the 2nd Best one !!
int min_err_idx2 = min_err_idx;
min_err = MAX_FLOAT;
for(i=0; i < sol_n; i++)
{
if(sol[i].obj_err < min_err &&
(min_err_idx2 != i) )
{
min_err = sol[i].obj_err;
min_err_idx = i;
}
}
// Copy the 2nd best solution !!!
if(min_err_idx >= 0)
{
mat33_copy(Rb, &sol[min_err_idx].PoseLu_R);
vec3_copy(tb, &sol[min_err_idx].PoseLu_t);
*errb = sol[min_err_idx].obj_err;
}
else
{
mat33_clear(R);
vec3_clear(t);
*err = MAX_FLOAT;
}
}
