vec3 tgErrorMetric::GetMean(const TomGine::tgPose &p1, const TomGine::tgPose &p2)
{
double err_x = 0.0;
double err_y = 0.0;
double err_z = 0.0;
double dN = 1.0 / pointlist.size();
for(unsigned i=0; i<pointlist.size(); i++){
mat3 R1, R2;
vec3 t1, t2;
p1.GetPose(R1,t1);
p2.GetPose(R2,t2);
vec3 verr = (R1 * pointlist[i] + t1) - (R2 * pointlist[i] + t2);
err_x += double(fabs(verr.x)) * dN;
err_y += double(fabs(verr.y)) * dN;
err_z += double(fabs(verr.z)) * dN;
}
vec3 err = vec3(err_x, err_y, err_z);
return err;
}
bool Recognizer3D::learnSifts(IplImage* tFrame, const TomGine::tgModel& m1, const TomGine::tgPose& pose)
{
P::Array<P::KeypointDescriptor*> m_tmp_keys;
mat3 R, Rt;
vec3 t;
TomGine::tgModel m2 = m1;
m_lastsiftexlist.clear();
// Transform model vertices to camera coordinats
pose.GetPose(R,t);
Rt = R.transpose();
for(unsigned int i=0; i<m2.m_vertices.size(); i++)
{
m2.m_vertices[i].pos = (R * m2.m_vertices[i].pos) + t;
m2.m_vertices[i].normal = R * m2.m_vertices[i].normal;
}
IplImage* tImg = 0;
IplImage* grey = 0;
float dcpfx = 1/m_cp.fx;
float dcpfy = 1/m_cp.fy;
tImg = cvCreateImage ( cvGetSize ( tFrame ), 8, 3 );
grey = cvCreateImage ( cvGetSize ( tFrame ), 8, 1 );
// undistort
cvRemap(tFrame, tImg, pMapX, pMapY );
// convert to gray scale image
cvConvertImage(tImg, grey);
//EXPERIMENTAL:
// cv::Mat descriptors;
// std::vector<cv::KeyPoint> keypoints;
// cv_detect->extract(cv::Mat(grey, true), descriptors, keypoints);
//
// pal_blort::CvDetect3D::cvKeypoints2BlortKeyPoints(
// descriptors, keypoints, m_image_keys);
// END OF EXPERIMENTAL
sift.Operate(grey, m_image_keys);
for(unsigned int i=0; i<m_image_keys.Size(); i++)
{
vec3 point, normal;
std::vector<vec3> pl; // point list
std::vector<vec3> nl; // normal list
std::vector<double> zl; // z-value list (depth)
float u,v,x,y;
u = (float)m_image_keys[i]->p.x;
v = (float)m_image_keys[i]->p.y;
x = (u-m_cp.cx) * dcpfx;
y = (v-m_cp.cy) * dcpfy;
// Create ray from pixel
TomGine::tgRay ray;
ray.start = vec3(0.0f,0.0f,0.0f);
ray.dir = vec3(x,y,1.0f);
ray.dir.normalize();
if( TomGine::tgCollission::IntersectRayModel(pl, nl, zl, ray, m2)
&& !pl.empty()
&& !zl.empty())
{
// determine nearest intersection point
unsigned int idx_min = 0;
float z_min = zl[0];
for(unsigned int idx=0; idx<zl.size(); idx++){
if(zl[idx] < z_min){
idx_min = idx;
z_min = zl[idx];
}
}
if(z_min > 0.0f){
// Transform to object space
point = (Rt * (pl[idx_min] - t));
normal = (Rt * nl[idx_min]);
vec3 r = (Rt * ray.dir) * zl[idx_min];
Siftex s;
s.pos = point;
s.normal = normal;
s.viewray = r;
normal.normalize();
r.normalize();
if( (r * normal) < -0.1f)
{
// Save sift
m_siftexlist.push_back(s);
m_lastsiftexlist.push_back(s);
m_image_keys[i]->SetPos(point.x, point.y, point.z);
m_tmp_keys.PushBack(m_image_keys[i]);
}
}
}
}
if(m_tmp_keys.Size() > 0)
{
m_sift_model_learner.AddToModel(m_tmp_keys, *m_sift_models[0]);
ROS_INFO("[Recognizer3D::learnSifts] added %d sifts to model\n", m_tmp_keys.Size());
m_model_loaded = true;
}
if(m_display){
display_image = cv::Mat(tImg, true);
}
cvReleaseImage(&tImg);
cvReleaseImage(&grey);
return true;
}