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