void CTomGineThread::AddSifts(const std::vector<blortRecognizer::Siftex>& sl)
{
m_mutex.Lock();
m_lastsiftexlist = sl;
for(unsigned i=0; i<sl.size(); i++)
m_siftexlist.push_back(sl[i]);
m_mutex.Unlock();
double pos[3] = {0,0,0};
double normal[3] = {0,0,0};
double view[3] = {0,0,0};
unsigned sc = sl.size();
for(unsigned i=0; i<sc; i++){
pos[0] += sl[i].pos.x;
pos[1] += sl[i].pos.y;
pos[2] += sl[i].pos.z;
normal[0] += sl[i].normal.x;
normal[1] += sl[i].normal.y;
normal[2] += sl[i].normal.z;
view[0] += sl[i].viewray.x;
view[1] += sl[i].viewray.y;
view[2] += sl[i].viewray.z;
}
vec3 vPos = vec3(float(pos[0]/sc), float(pos[1]/sc), float(pos[2]/sc));
//vec3 vNormal = vec3(float(normal[0]/sc), float(normal[1]/sc), float(normal[2]/sc));
vec3 vView = vec3(float(view[0]/sc), float(view[1]/sc), float(view[2]/sc));
vec3 p = vPos - vView;
//vec3 p = sl[0].pos - sl[0].viewray;
vec3 f = vView; f.normalize();
vec3 u = vec3(0,0,1);
vec3 s;
s.cross(u,f); s.normalize();
u.cross(f,s); u.normalize();
float rad = 0.0f;
for(unsigned i=0; i<sc; i++){
vec3 d = vPos - sl[i].pos;
float l = d.length();
if(rad < l)
rad=l;
}
m_viewscale.push_back(vec3(rad, rad, vView.length()));
float fR[9] = { s.x, s.y, s.z,
u.x, u.y, u.z,
f.x, f.y, f.z};
mat3 R = mat3(fR);
TomGine::tgPose pose;
pose.SetPose(R, p);
m_viewlist.push_back(pose);
}
void Recognizer3D::Convert(P::PoseCv& p1, TomGine::tgPose& p2){
mat3 cR;
vec3 ct;
cR[0] = cvmGet(p1.R,0,0); cR[3] = cvmGet(p1.R,0,1); cR[6] = cvmGet(p1.R,0,2);
cR[1] = cvmGet(p1.R,1,0); cR[4] = cvmGet(p1.R,1,1); cR[7] = cvmGet(p1.R,1,2);
cR[2] = cvmGet(p1.R,2,0); cR[5] = cvmGet(p1.R,2,1); cR[8] = cvmGet(p1.R,2,2);
ct.x = cvmGet(p1.t,0,0);
ct.y = cvmGet(p1.t,1,0);
ct.z = cvmGet(p1.t,2,0);
p2.SetPose(cR,ct);
}
void getCamPose(const char* pose_cal_file, TomGine::tgPose& camPose){
CDataFile cdfParams;
if(!cdfParams.Load(pose_cal_file)){
char errmsg[128];
sprintf(errmsg, "[utilities::getCamPose] Can not open pose_cal file '%s'", pose_cal_file);
throw std::runtime_error(errmsg);
}
vec3 t, r;
mat3 R;
std::string pose = cdfParams.GetString("pose");
sscanf( pose.c_str(), "[%f %f %f] [%f %f %f]",
&(t.x), &(t.y), &(t.z), &(r.x), &(r.y), &(r.z) );
R.fromRotVector(r);
camPose.SetPose(R,t);
}
void getCamPoseCvCheckerboard(const cv::Mat_<double> &R, const cv::Mat_<double> &T, TomGine::tgPose &camPose)
{
vec3 r, t;
mat3 Rm;
cv::Mat_<double> cv_Rm = cv::Mat_<double> ( 3,3 );
cv::Mat_<double> cv_rvec = cv::Mat_<double> ( 3,1 );
cv::Mat_<double> cv_tvec = cv::Mat_<double> ( 3,1 );
cv::Rodrigues(R, cv_Rm);
cv::transpose(cv_Rm, cv_Rm);
cv_tvec = -cv_Rm * T; // t = -R^T * t + t
cv::Rodrigues(cv_Rm, cv_rvec);
r.x = cv_rvec(0,0);
r.y = cv_rvec(1,0);
r.z = cv_rvec(2,0);
t.x = cv_tvec(0,0);
t.y = cv_tvec(1,0);
t.z = cv_tvec(2,0);
Rm.fromRotVector(r);
camPose.SetPose(Rm,t);
}