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); } } } }