void MatchSiftFeatures( MatchArray& aryMatch, const CFeatureArray& set1, const CFeatureArray& set2 ) { aryMatch.resize( set1.size() ); int num = 0; // for each feature in set1 for( int i=0; i<(int)set1.size(); ++i ) { if( i%100 == 0 ) printf( "%d / %d\n", i, set1.size() ); // const CSiftFeature* ft1 = dynamic_cast< const CSiftFeature* >( set1[i] ); int j = FindMatch( ft1, set2 ); int k = FindMatch( set2[j], set1 ); bool bQualMatch = false; if( i==k ) bQualMatch = true; // TODO: Find the best match for ft1 in set2. // The quality of a match can be evaluated. We only // collect matches of good qualities. if( bQualMatch ) aryMatch[ num++ ] = make_pair( i, j ); } aryMatch.resize( num ); }
// ------------------------------------------------------------------- bool PosixRegEx::execute(MatchArray &sub, const String &str, size_t index, size_t count, int eflags) { if( !compiled) { BLOCXX_THROW(RegExCompileException, "Regular expression is not compiled"); } if( index > str.length()) { BLOCXX_THROW(OutOfBoundsException, Format("String index out of bounds (" "length = %1, index = %2).", str.length(), index ).c_str()); } if( count == 0) { count = m_regex.re_nsub + 1; } AutoPtrVec<regmatch_t> rsub(new regmatch_t[count]); rsub[0].rm_so = -1; rsub[0].rm_eo = -1; sub.clear(); m_ecode = ::regexec(&m_regex, str.c_str() + index, count, rsub.get(), eflags); if( m_ecode == REG_NOERROR) { m_error.erase(); if( m_flags & REG_NOSUB) { return true; } sub.resize(count); for(size_t n = 0; n < count; n++) { if( rsub[n].rm_so < 0 || rsub[n].rm_eo < 0) { sub[n] = rsub[n]; } else { rsub[n].rm_so += index; rsub[n].rm_eo += index; sub[n] = rsub[n]; } } return true; } else { m_error = getError(&m_regex, m_ecode); return false; } }
void GetHomographyInliers( MatchArray& aryInlier, const MatchArray& aryMatch, const CFeatureArray& set1, const CFeatureArray& set2, const CHomography& h**o, float tol ) { float SQR_TOL = tol*tol; aryInlier.resize( aryMatch.size() ); int k=0; for( int i=0; i<aryMatch.size(); ++i ) { const CFeature* ft1 = set1[ aryMatch[i].first ]; const CFeature* ft2 = set2[ aryMatch[i].second ]; CvPoint2D64f pt = h**o * cvPoint2D64f( ft1->x, ft1->y ); double dx = pt.x -ft2->x; double dy = pt.y -ft2->y; if( dx*dx +dy*dy < SQR_TOL ) // a homography inlier aryInlier[k++] = aryMatch[i]; } aryInlier.resize(k); }
// ------------------------------------------------------------------- StringArray PosixRegEx::split(const String &str, bool empty, int eflags) { if( !compiled) { BLOCXX_THROW(RegExCompileException, "Regular expression is not compiled"); } MatchArray rsub; StringArray ssub; bool match; size_t off = 0; size_t len = str.length(); do { match = execute(rsub, str, off, 1, eflags); if( match) { if( rsub.empty() || rsub[0].rm_so < 0 || rsub[0].rm_eo < 0) { BLOCXX_THROW(RegExCompileException, "Non-capturing regular expression"); } if( empty || ((size_t)rsub[0].rm_so > off)) { ssub.push_back(str.substring(off, rsub[0].rm_so - off)); } off = rsub[0].rm_eo; } else if(m_ecode == REG_NOMATCH) { String tmp = str.substring(off); if( empty || !tmp.empty()) { ssub.push_back(tmp); } m_ecode = REG_NOERROR; m_error.erase(); } else { BLOCXX_THROW_ERR(RegExExecuteException, errorString().c_str(), m_ecode); } } while(match && len > off); return ssub; }
// ------------------------------------------------------------------- blocxx::String PosixRegEx::replace(const String &str, const String &rep, bool global, int eflags) { if( !compiled) { BLOCXX_THROW(RegExCompileException, "Regular expression is not compiled"); } MatchArray rsub; bool match; size_t off = 0; String out = str; do { match = execute(rsub, out, off, 0, eflags); if( match) { if( rsub.empty() || rsub[0].rm_so < 0 || rsub[0].rm_eo < 0) { // only if empty (missused as guard). BLOCXX_THROW(RegExCompileException, "Non-capturing regular expression"); } String res = substitute_caps(rsub, out, rep); out = out.substring(0, rsub[0].rm_so) + res + out.substring(rsub[0].rm_eo); off = rsub[0].rm_so + res.length(); } else if(m_ecode == REG_NOMATCH) { m_ecode = REG_NOERROR; m_error.erase(); } else { BLOCXX_THROW_ERR(RegExExecuteException, errorString().c_str(), m_ecode); } } while(global && match && out.length() > off); return out; }
// ------------------------------------------------------------------- StringArray PosixRegEx::capture(const String &str, size_t index, size_t count, int eflags) { if( !compiled) { BLOCXX_THROW(RegExCompileException, "Regular expression is not compiled"); } MatchArray rsub; StringArray ssub; bool match = execute(rsub, str, index, count, eflags); if( match) { if( rsub.empty()) { BLOCXX_THROW(RegExCompileException, "Non-capturing regular expression"); } MatchArray::const_iterator i=rsub.begin(); for( ; i != rsub.end(); ++i) { if( i->rm_so >= 0 && i->rm_eo >= 0) { ssub.push_back(str.substring(i->rm_so, i->rm_eo - i->rm_so)); } else { ssub.push_back(String("")); } } } else if(m_ecode != REG_NOMATCH) { BLOCXX_THROW_ERR(RegExExecuteException, errorString().c_str(), m_ecode); } return ssub; }
/** * OptimizePair: * Input: * cam1 - the first camera (already optimized) * cam2 - the second camera with its intrinsic matrix initialized * dR - an initial relative 3x3 camera rotation matrix * set1 - SIFT features in the first image * set2 - SIFT features in the second image * aryInlier - the homography iniliers * * Ouput: * cam2 - update cam2's optimized focal length and pose */ void OptimizeSingle( const CCamera& cam1, CCamera& cam2, double* dR, const CFeatureArray& set1, const CFeatureArray& set2, const MatchArray& aryInlier ) { // Step 1. Initialize the camera pose of cam2 // cam2's relative rotation to cam1 CvMat matR = cvMat( 3, 3, CV_64F, dR ); // cam1's absolute rotation double dRod1[3]; CvMat matRod1 = cvMat( 3, 1, CV_64F, dRod1 ); cam1.GetPose( dRod1 ); double dRot1[9]; CvMat matRot1 = cvMat( 3, 3, CV_64F, dRot1 ); cvRodrigues2( &matRod1, &matRot1 ); // compose R and Rot1 to get cam2's initial absolute rotation cvMatMul( &matR, &matRot1, &matR ); double dRod2[3]; CvMat matRod2 = cvMat( 3, 1, CV_64F, dRod2 ); cvRodrigues2( &matR, &matRod2 ); cam2.SetPose( dRod2 ); // Step 2. Now we can perform bundle adjustment for cam2 CBundleAdjust ba( 1, BA_ITER ); ba.SetCamera( &cam2, 0 ); // set points for( int i=0; i<aryInlier.size(); ++i ) { const CFeature* ft1 = set1[ aryInlier[i].first ]; const CFeature* ft2 = set2[ aryInlier[i].second ]; double dir[3]; cam1.GetRayDirectionWS( dir, cvPoint2D64f( ft1->x, ft1->y ) ); // the 3d position CvPoint3D64f pt3 = cvPoint3D64f( dir[0]*radius, dir[1]*radius, dir[2]*radius ); ba.SetPointProjection( pt3, 0, cvPoint2D64f( ft2->x, ft2->y ) ); } ba.DoMotion(); ba.GetAdjustedCamera( &cam2, 0 ); }
/** * OptimizePair: * Input: * cam1 - the first camera with its intrinsic matrix and pose initialized ([R|T]=[I|0]) * cam2 - the second camera with its intrinsic matrix initialized * dR - an initial relative 3x3 camera rotation matrix * set1 - SIFT features in the first image * set2 - SIFT features in the second image * aryInlier - the homography iniliers * * Ouput: * cam1 - update cam1's optimized folcal length * cam2 - update cam2's optimized focal length and pose */ void OptimizePair( CCamera& cam1, CCamera& cam2, double* dR, const CFeatureArray& set1, const CFeatureArray& set2, const MatchArray& aryInlier ) { CBundleAdjust ba( 2, BA_ITER ); // Step 1. To perform bundle adjustment, we initialize cam1 and cam2 // as [K][I|0} and [K][R|0] respectively and optimize R using // bundle adjustment. double dRod[3]; CvMat matRod = cvMat( 3, 1, CV_64F, dRod ); CvMat matR = cvMat( 3, 3, CV_64F, dR ); cvRodrigues2( &matR, &matRod ); cam2.SetPose( dRod ); // Set cameras ba.SetCamera( &cam1, 0 ); ba.SetCamera( &cam2, 1 ); // Step 2. We still need to create a set of 3D points. From each homography inlier, // a 3D point can be initialized by locating it on the ray that goes through // its projection. for( int i=0; i<aryInlier.size(); ++i ) { const CFeature* ft1 = set1[ aryInlier[i].first ]; const CFeature* ft2 = set2[ aryInlier[i].second ]; double dir[3]; cam1.GetRayDirectionWS( dir, cvPoint2D64f( ft1->x, ft1->y ) ); // the initialized 3d position CvPoint3D64f pt3 = cvPoint3D64f( dir[0]*radius, dir[1]*radius, dir[2]*radius ); // set the 3d point and its projections in both images ba.SetPointProjection( pt3, 0, cvPoint2D64f( ft1->x, ft1->y ) ); ba.SetPointProjection( pt3, 1, cvPoint2D64f( ft2->x, ft2->y ) ); } // perform bundle adjustment ba.DoMotionAndStructure(); // retrieve the optimized cameras ba.GetAdjustedCamera( &cam1, 0 ); ba.GetAdjustedCamera( &cam2, 1 ); }
/** * RansacHomography: * Input: * aryMatch - an array of potential matches between two images * inlierTol - the tolerance to regard a match as an inlier * numIter - number of iterations for Ransac * * Ouput: * h**o - the best estimated homography (with the max nubmer of inliers) */ void RansacHomography( CHomography& h**o, const MatchArray& aryMatch, const CFeatureArray& set1, const CFeatureArray& set2, float inlierTol, int numIter ) { const float SQR_TOL = inlierTol*inlierTol; const int NUM_SAMP = 6; int maxInlier = 0; double dA[ NUM_SAMP*2*8 ]; double dB[ NUM_SAMP*2 ]; // ToDo2: Find homography using RANSAC printf( "homography inliers: %d(%d)\n", maxInlier, aryMatch.size() ); }
/** * RansacHomography: * Input: * aryMatch - an array of potential matches between two images * inlierTol - the tolerance to regard a match as an inlier * numIter - number of iterations for Ransac * * Ouput: * h**o - the best estimated homography (with the max nubmer of inliers) */ void RansacHomography( CHomography& h**o, const MatchArray& aryMatch, const CFeatureArray& set1, const CFeatureArray& set2, float inlierTol, int numIter ) { const float SQR_TOL = inlierTol*inlierTol; const int NUM_SAMP = 6; int maxInlier = 0; double dA[ NUM_SAMP*2*8 ]; double dB[ NUM_SAMP*2 ]; double dH[8]; struct timeval tv; gettimeofday(&tv, NULL); srand48(tv.tv_usec); long int index; int flag; vector < int > dup_index; int test_index[] = {100, 150, 200, 250, 300, 350}; // ToDo2: Find homography using RANSAC for(int i=0; i < NUM_SAMP; i++) { dB[i] = 0; } for(int i=0; i< numIter; i++) { dup_index.clear(); for(int j =0 ; j< NUM_SAMP; j++ ) { index = lrand48(); index = index % aryMatch.size(); // index = test_index[j]; flag = 1; for(unsigned int k=0; k < dup_index.size(); k++) { if(dup_index[k] == index) { flag = 0; } } if(!flag) { --j; continue; } // std::cout << "Index " << index << std::endl; int ind1 = aryMatch[index].first; int ind2 = aryMatch[index].second; dA[16*j + 0] = -1 * set1[ind1]->x; dA[16*j + 1] = -1 * set1[ind1]->y; dA[16*j + 2] = -1; dA[16*j + 3] = 0; dA[16*j + 4] = 0; dA[16*j + 5] = 0; dA[16*j + 6] = set2[ind2]->x * set1[ind1]->x; dA[16*j + 7] = set2[ind2]->x * set1[ind1]->y; dA[16*j + 8 + 0] = 0; dA[16*j + 8 + 1] = 0; dA[16*j + 8 + 2] = 0; dA[16*j + 8 + 3] = -1 * set1[ind1]->x; dA[16*j + 8 + 4] = -1 * set1[ind1]->y; dA[16*j + 8 + 5] = -1; dA[16*j + 8 + 6] = set2[ind2]->y * set1[ind1]->x; dA[16*j + 8 + 7] = set2[ind2]->y * set1[ind1]->y; dB[2*j] = -1*set2[ind2]->x; dB[2*j + 1] = -1*set2[ind2]->y; } for(int j = 0; j < 8; j++) dH[j] = 0; CvMat mA = cvMat( NUM_SAMP * 2, 8, CV_64FC1, dA); CvMat mB = cvMat( NUM_SAMP * 2, 1, CV_64FC1, dB); CvMat mH = cvMat( 8, 1, CV_64FC1, dH); int ret = cvSolve(&mA, &mB, &mH, CV_SVD); // std::cout << ret << std::endl; for(int j = 0; j < 8; j++) { dH[j] = cvmGet(&mH, j, 0); // std::cout << "H is " << std::endl; // std::cout << dH[j] << std::endl; } //continue; int inlier_count = 0; for(unsigned int j = 0; j< aryMatch.size(); j++) { int ind1 = aryMatch[j].first; int ind2 = aryMatch[j].second; double x2 = set2[ind2]->x; double y2 = set2[ind2]->y; double x1 = set1[ind1]->x; double y1 = set1[ind1]->y; double _x1p = dH[0] * x1 + dH[1] * y1 + dH[2]*1; double _y1p = dH[3] * x1 + dH[4] * y1 + dH[5]*1; double h1p = dH[6] * x1 + dH[7] * y1 + 1; double x1p = _x1p / h1p; double y1p = _y1p / h1p; double dist = (x1p - x2)*(x1p - x2) + (y1p - y2)*(y1p - y2); // std::cout << "Distance is " << sqrt(dist) << std::endl; if( sqrt(dist) < SQR_TOL) { // std::cout << "Coming here" << std::endl; inlier_count++; } } if(inlier_count > maxInlier) { maxInlier = inlier_count; for(int k = 0; k < 8; k++) h**o.d[k] = dH[k]; h**o.d[8] = 1; } } printf( "homography inliers: %d(%d)\n", maxInlier, aryMatch.size() ); }