void computeHomography(
const vector<Vector3d>& f_ref,
const vector<Vector3d>& f_cur,
double focal_length,
double reprojection_threshold,
vector<int>& inliers,
vector<Vector3d>& xyz_in_cur,
SE3& T_cur_from_ref)
{
vector<Vector2d, aligned_allocator<Vector2d> > uv_ref(f_ref.size());
vector<Vector2d, aligned_allocator<Vector2d> > uv_cur(f_cur.size());
for(size_t i=0, i_max=f_ref.size(); i<i_max; ++i)
{
uv_ref[i] = vk::project2d(f_ref[i]);
uv_cur[i] = vk::project2d(f_cur[i]);
}
vk::Homography Homography(uv_ref, uv_cur, focal_length, reprojection_threshold);
Homography.computeSE3fromMatches();
vector<int> outliers;
vk::computeInliers(f_cur, f_ref,
Homography.T_c2_from_c1.rotation_matrix(), Homography.T_c2_from_c1.translation(),
reprojection_threshold, focal_length,
xyz_in_cur, inliers, outliers);
T_cur_from_ref = Homography.T_c2_from_c1;
}
Picture UnStabilizator::nextStep(Picture basePic)
{
if (basePic.getWidth() == 0) return Picture();
double angle = dif_lib::randomDoubleSigned(shiftAngle * M_PI / 180.0);
double* homoData = new double[6];
homoData[2] = dif_lib::randomDoubleSigned(shiftX);
homoData[5] = dif_lib::randomDoubleSigned(shiftY);
homoData[0] = cos(angle);
homoData[4] = cos(angle);
homoData[1] = sin(angle);
homoData[3] = -sin(angle);
Homography h**o = Homography(Matrix(2,3,sh_ptr_db(homoData)));
basePic = picConverter.applyHomo(basePic, h**o);
return basePic;
}
bool THIS::computeHomography()
{
double homMat[9];
CvMat homMatCv;
memset ( homMat, 0, 9*sizeof ( double ) );
homMatCv = cvMat ( 3, 3, CV_64F, homMat );
std::vector<CvPoint2D32f> points1Cv, points2Cv;
CvMat points1CvMat, points2CvMat;
int numMatches = m_Matches.size();
if ( numMatches < 4 )
{
return false;
}
// Set vectors to correct size
points1Cv.resize ( numMatches );
points2Cv.resize ( numMatches );
// Copy Ipoints from match vector into cvPoint vectors
std::list<KeyPointMatch>::iterator currentMatch = m_Matches.begin();
int i = 0;
while ( currentMatch != m_Matches.end() )
{
points1Cv[i] = cvPoint2D32f ( ( *m_KeyPoints1 ) [ currentMatch->index1 ].x,
( *m_KeyPoints1 ) [ currentMatch->index1 ].y );
points2Cv[i] = cvPoint2D32f ( ( *m_KeyPoints2 ) [ currentMatch->index2 ].x,
( *m_KeyPoints2 ) [ currentMatch->index2 ].y );
currentMatch++;
i++;
}
points1CvMat = cvMat ( 1, numMatches, CV_32FC2, &points1Cv[0] );
points2CvMat = cvMat ( 1, numMatches, CV_32FC2, &points2Cv[0] );
// Find the homography (transformation) between the two sets of points
//0 - regular method using all the point pairs
//CV_RANSAC - RANSAC-based robust method
//CV_LMEDS - Least-Median robust method
int method = 0;
switch (CV_RANSAC)//Config::getInstance()->getInt( "ObjectRecognition.Homography.iMethod" ))
{
case 0 :
method = 0;
break;
case 1 :
method = CV_RANSAC;
break;
case 2 :
method = CV_LMEDS;
break;
default:
//TRACE_ERROR("Undefined methode to find homography"); // TODO use ros
break;
}
m_Success = cvFindHomography( &points2CvMat, &points1CvMat, &homMatCv, method, m_MaxReprojectionError );
// float n=sqrt(homMat[0]*homMat[0]+homMat[3]*homMat[3]) * sqrt(homMat[1]*homMat[1]+homMat[4]*homMat[4]);
//
// float det = homMat[0]*homMat[4] - homMat[1]*homMat[3];
// det /= n;
// float l = fabs( det );
//
// if ( l < 0.8 )
// {
// m_Success = false;
// }
//
// TRACE_INFO( "det: " << det );
//
// /*
// float scalar= homMat[0]*homMat[1] + homMat[3]*homMat[4];
// scalar /= n;
// */
m_Homography = Homography( homMat );
return m_Success;
}
void MVT_3D_Object::Project_to_2D(MVT_2D_Object* ret_object2d, MVT_Viewpoint viewpoint)
{
MVT_Viewpoint_IDX idx_viewpoint = DiscViewpoint_from_ContViewpoint(viewpoint);
cv::Mat mat_P = Projection(viewpoint);
double m_R[3][3] = { {1, 0, 0.5},
{0, -1, 0.5},
{0, 0, 1} };
for(unsigned int pv=0 ; pv<m_num_of_partsNroots ; pv++)
{
ret_object2d->SetOccluded(pv,m_is_occluded[idx_viewpoint.a][idx_viewpoint.e][idx_viewpoint.d][pv]);
/* if( ret_object2d->IsOccluded(pv) )
{
}
else
*/ {
if(
( m_object_category==OBJECT_CATEGORY_CHAIR && pv>10 ) ||
( m_object_category==OBJECT_CATEGORY_TABLE && pv>5 )
)
{
// TODO
// note render.m
}
else
{
cv::Mat mat_R = m_2dparts_front[pv].viewport * cv::Mat(3,3,CV_64F, m_R);
mat_R.at<double>(2,2)=1;
if( pv < m_num_of_parts )
{
unsigned int n_vertices = m_3dparts[pv].vertices.size();
double m[4][n_vertices];
for( unsigned int v=0; v<n_vertices; v++ )
{
m[0][v] = m_3dparts[pv].vertices[v].x;
m[1][v] = m_3dparts[pv].vertices[v].y;
m[2][v] = m_3dparts[pv].vertices[v].z;
m[3][v] = 1;
}
cv::Mat part = mat_P*cv::Mat(4,n_vertices,CV_64F,m);
for( unsigned int v=0; v<n_vertices; v++ )
{
// normalize
double tmp = part.at<double>(3,v);
for( unsigned int r=0; r<4; r++ )
{
part.at<double>(r,v) = part.at<double>(r,v) / tmp;
}
}
double m_vertices2d[3][n_vertices];
for( unsigned int v=0; v<n_vertices; v++ )
{
m_vertices2d[0][v] = part.at<double>(0,v);
m_vertices2d[1][v] = part.at<double>(1,v);
m_vertices2d[2][v] = 1;
}
cv::Mat mat_v = mat_R*cv::Mat(3,n_vertices,CV_64F, m_vertices2d);
double m_center3d[4] = { m_3dparts[pv].center.x,
m_3dparts[pv].center.y,
m_3dparts[pv].center.z,
1 };
cv::Mat center = mat_P*cv::Mat(4,1,CV_64F, m_center3d);
center = center * (1/center.at<double>(3));// normalize
double m_center2d[3] = { center.at<double>(0),
center.at<double>(1),
1 };
cv::Mat mat_c = mat_R*cv::Mat(3,1,CV_64F, m_center2d);
ret_object2d->m_2dparts[pv].vertices.clear();
ret_object2d->m_2dparts[pv].vertices.resize(n_vertices);
for( unsigned int v=0; v<n_vertices; v++ )
{
ret_object2d->m_2dparts[pv].vertices[v].x = mat_v.at<double>(0,v) - mat_c.at<double>(0);
ret_object2d->m_2dparts[pv].vertices[v].y = mat_v.at<double>(1,v) - mat_c.at<double>(1);
}
ret_object2d->m_2dparts[pv].center.x
= mat_c.at<double>(0) - mat_R.at<double>(0,2);
ret_object2d->m_2dparts[pv].center.y
= mat_c.at<double>(1) - mat_R.at<double>(1,2);
}
else
{
int x_max=-INFINITY, x_min=INFINITY, y_max=-INFINITY, y_min=INFINITY;
for( unsigned int p=0; p<m_num_of_parts; p++ )
{
unsigned int n_vertices = ret_object2d->m_2dparts[p].vertices.size();
for( unsigned int v=0; v<n_vertices; v++ )
{
int x = ret_object2d->m_2dparts[p].vertices[v].x + ret_object2d->m_2dparts[p].center.x;
int y = ret_object2d->m_2dparts[p].vertices[v].y + ret_object2d->m_2dparts[p].center.y;
x_max = (x > x_max) ? x : x_max;
x_min = (x < x_min) ? x : x_min;
y_max = (y > y_max) ? y : y_max;
y_min = (y < y_min) ? y : y_min;
}
}
ret_object2d->m_2dparts[pv].vertices.clear();
ret_object2d->m_2dparts[pv].vertices.resize(5);
ret_object2d->m_2dparts[pv].vertices[0] = cv::Point2d(x_min, y_min);
ret_object2d->m_2dparts[pv].vertices[1] = cv::Point2d(x_min, y_max);
ret_object2d->m_2dparts[pv].vertices[2] = cv::Point2d(x_max, y_max);
ret_object2d->m_2dparts[pv].vertices[3] = cv::Point2d(x_max, y_min);
ret_object2d->m_2dparts[pv].vertices[4] = cv::Point2d(x_min, y_min);
ret_object2d->m_2dparts[pv].center.x = 0;
ret_object2d->m_2dparts[pv].center.y = 0;
}
ret_object2d->m_homography_part2front[pv].release();
ret_object2d->m_homography_part2front[pv]
= Homography(ret_object2d->m_2dparts[pv].vertices,m_2dparts_front[pv].vertices);
}
}
}
}
