bool VisualOdometry::addFrame ( Frame::Ptr frame )
{
    switch ( state_ )
    {
    case INITIALIZING:
    {
        state_ = OK;
        curr_ = ref_ = frame;
        map_->insertKeyFrame ( frame );
        // extract features from first frame and add them into map
        extractKeyPoints();
        computeDescriptors();
        setRef3DPoints();
        break;
    }
    case OK:
    {
        curr_ = frame;
        extractKeyPoints();
        computeDescriptors();
        featureMatching();
        poseEstimationPnP();
        if ( checkEstimatedPose() == true ) // a good estimation
        {
            curr_->T_c_w_ = T_c_r_estimated_ * ref_->T_c_w_;  // T_c_w = T_c_r*T_r_w 
            ref_ = curr_;
            setRef3DPoints();
            num_lost_ = 0;
            if ( checkKeyFrame() == true ) // is a key-frame
            {
                addKeyFrame();
            }
        }
        else // bad estimation due to various reasons
        {
            num_lost_++;
            if ( num_lost_ > max_num_lost_ )
            {
                state_ = LOST;
            }
            return false;
        }
        break;
    }
    case LOST:
    {
        cout<<"vo has lost."<<endl;
        break;
    }
    }

    return true;
}
Esempio n. 2
0
bool SURF_OCL::detectAndCompute(InputArray _img, InputArray _mask, UMat& keypoints,
                                OutputArray _descriptors, bool useProvidedKeypoints )
{
    if( !setImage(_img, _mask) )
        return false;

    if( !useProvidedKeypoints && !detectKeypoints(keypoints) )
        return false;

    return computeDescriptors(keypoints, _descriptors);
}
Esempio n. 3
0
void computeDescriptorsLane(void *pixels, int depth, int width, int height, VertexBufferObject &points, DescriptorData  &descriptors)
{
    unsigned char *srcBuffer;
    unsigned char *dstBuffer;
    cudaMalloc( (void**) &srcBuffer, sizeof(unsigned char)*depth*width*height);
    cudaMemcpy( srcBuffer, pixels, sizeof(unsigned char)*depth*width*height, cudaMemcpyHostToDevice );
    cudaMalloc((void**) &dstBuffer, sizeof(unsigned char)*depth*width*height*2);
    // convert to RGB
    cudaYCbYCrToY( (uchar4*)dstBuffer, (uchar4*)srcBuffer, width*2, height);
    std::cout << "Passed cudaYCbYCrToY" << std::endl;

    // convert to 1 plane float cuda
    //float *yBuffer;
    //cudaMalloc((void**)&yBuffer, sizeof(float)*width*2*height);
    
    UInt2 imgSize(1920,1080);
    CudaImageBuffer<float> m_satImage;
    allocBuffer(m_satImage, imgSize);
    
    cudaRGBAtoCuda((float*)m_satImage, (uchar4*)dstBuffer, width*2, height, width*2); 
    std::cout << "Passed cudaRGBAtoCuda" << std::endl;
    
    convertToIntegral(m_satImage);
    std::cout << "Passed convertToIntegral" << std::endl;
    
    CudaImageBuffer<float> m_hesImage;
    allocBuffer(m_hesImage, imgSize);
    HessianData     m_hessianData;
    m_hessianData.allocImages(imgSize);
    computeHessianDet( m_satImage, m_hesImage, m_hessianData );
    std::cout << "Passed computeHessianDet" << std::endl;

    computeNonMaxSuppression( m_hesImage, m_hessianData );
    std::cout << "Passed computeNonMaxSuppression" << std::endl;

    collectHessianPoints( m_hessianData, descriptors);
    std::cout << "Passed collectHessianPoint" << std::endl;
    
    computeDescriptors( m_satImage, descriptors);
    std::cout << "Passed computeDescriptors" << std::endl;

    //collectPoints( descriptors, points, imgSize );
    
    // free memory
    m_hessianData.freeImages();
    releaseBuffer(m_hesImage); 
    releaseBuffer(m_satImage);
    cudaFree(srcBuffer);
    cudaFree(dstBuffer);
}
Esempio n. 4
0
void LDB::compute( const cv::Mat& image,
                   std::vector<cv::KeyPoint>& _keypoints,
                   cv::Mat& _descriptors,
				   bool flag) const
{
    // Added to work with color images.
    cv::Mat _image;
    image.copyTo(_image);

	if(_image.empty() )
		return;

	//ROI handling
	int halfPatchSize = patchSize / 2;
	int border = halfPatchSize*1.415 + 1;
	if( _image.type() != CV_8UC1 )
		cvtColor(_image, _image, CV_BGR2GRAY);
	int levelsNum = 0;
	for( size_t i = 0; i < _keypoints.size(); i++ )
		levelsNum = std::max(levelsNum, std::max(_keypoints[i].octave, 0));
	levelsNum++;
	
	//Compute Orientation
    if(flag == 1){
       computeOrientation(_image, _keypoints, halfPatchSize);
    }

	// Pre-compute the scale pyramids
    std::vector<cv::Mat> imagePyramid(levelsNum);
	for (int level = 0; level < levelsNum; ++level)
	{
		float scale = 1/getScale(level, firstLevel, scaleFactor);
        cv::Size sz(cvRound(_image.cols*scale), cvRound(_image.rows*scale));
        cv::Size wholeSize(sz.width + border*2, sz.height + border*2);
        cv::Mat temp(wholeSize, _image.type()), masktemp;
        imagePyramid[level] = temp(cv::Rect(border, border, sz.width, sz.height));

		// Compute the resized image
		if( level != firstLevel )
		{
			if( level < firstLevel )
                resize(_image, imagePyramid[level], sz, 0, 0, cv::INTER_LINEAR);
			else
                resize(imagePyramid[level-1], imagePyramid[level], sz, 0, 0, cv::INTER_LINEAR);

            cv::copyMakeBorder(imagePyramid[level], temp, border, border, border, border,
                cv::BORDER_REFLECT_101+cv::BORDER_ISOLATED);
		}
		else
            cv::copyMakeBorder(_image, temp, border, border, border, border,
                cv::BORDER_REFLECT_101);
	}

	// Pre-compute the keypoints (we keep the best over all scales, so this has to be done beforehand
    std::vector < std::vector<cv::KeyPoint> > allKeypoints;

	// Cluster the input keypoints depending on the level they were computed at
	allKeypoints.resize(levelsNum);
    for (std::vector<cv::KeyPoint>::iterator keypoint = _keypoints.begin(),
		keypointEnd = _keypoints.end(); keypoint != keypointEnd; ++keypoint)
		allKeypoints[keypoint->octave].push_back(*keypoint);

	// Make sure we rescale the coordinates
	for (int level = 0; level < levelsNum; ++level)
	{
		if (level == firstLevel)
			continue;

        std::vector<cv::KeyPoint> & keypoints = allKeypoints[level];
		float scale = 1/getScale(level, firstLevel, scaleFactor);
        for (std::vector<cv::KeyPoint>::iterator keypoint = keypoints.begin(),
			keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
			keypoint->pt *= scale;
	}

    cv::Mat descriptors;
    std::vector<cv::Point> pattern;

	int nkeypoints = 0;
	for (int level = 0; level < levelsNum; ++level){
        std::vector<cv::KeyPoint>& keypoints = allKeypoints[level];
        cv::Mat& workingmat = imagePyramid[level];
		if(keypoints.size() > 1)
            cv::KeyPointsFilter::runByImageBorder(keypoints, workingmat.size(), border);

		nkeypoints += keypoints.size();
	}
	if( nkeypoints == 0 )
		_descriptors.release();
	else
	{
		_descriptors.create(nkeypoints, descriptorSize(), CV_8U);
		descriptors = _descriptors;
	}

	_keypoints.clear();
	int offset = 0;
	for (int level = 0; level < levelsNum; ++level)
	{
		// preprocess the resized image
        cv::Mat& workingmat = imagePyramid[level];
		// Get the features and compute their orientation
        std::vector<cv::KeyPoint>& keypoints = allKeypoints[level];
		if(keypoints.size() > 1)
            cv::KeyPointsFilter::runByImageBorder(keypoints, workingmat.size(), border);
		int nkeypoints = (int)keypoints.size();

		// Compute the descriptors
        cv::Mat desc;
		if (!descriptors.empty())
		{
			desc = descriptors.rowRange(offset, offset + nkeypoints);
		}

		offset += nkeypoints;
        //boxFilter(working_cv::Mat, working_cv::Mat, working_cv::Mat.depth(), Size(5,5), Point(-1,-1), true, BORDER_REFLECT_101);
        GaussianBlur(workingmat, workingmat, cv::Size(7, 7), 2, 2, cv::BORDER_REFLECT_101);
        cv::Mat integral_image;
        integral(workingmat, integral_image, CV_32S);
        computeDescriptors(workingmat, integral_image, patchSize, keypoints, desc, descriptorSize(), flag);

		// Copy to the output data
		if (level != firstLevel)
		{
			float scale = getScale(level, firstLevel, scaleFactor);
            for (std::vector<cv::KeyPoint>::iterator keypoint = keypoints.begin(),
				keypointEnd = keypoints.end(); keypoint != keypointEnd; ++keypoint)
				keypoint->pt *= scale;
		}
		// And add the keypoints to the output
		_keypoints.insert(_keypoints.end(), keypoints.begin(), keypoints.end());
	}
	
}