Exemplo n.º 1
0
  MLPTesting(): c_(&camera_){
    static_assert(imgSize_*dx_*dy_<255,"imgSize to large for gradients");
    pixel_ = cv::Point2f(patchSize_/2+1,patchSize_/2+1);
    bearing_ = V3D(patchSize_/2+1,patchSize_/2+1,1);
    bearing_.normalize();
    c_.set_c(pixel_);
    warp_c_ << 0.1, 0.5, 0.7, -0.2;
    c_.set_warp_c(warp_c_);
    warp_nor_ = c_.get_warp_nor();

    stat_.localQualityRange_ = 3;
    stat_.localVisibilityRange_ = 4;
    stat_.minGlobalQualityRange_ = 5;

    img1_ = cv::Mat::zeros(imgSize_,imgSize_,CV_8UC1);
    uint8_t* img_ptr = (uint8_t*) img1_.data;
    for(int i=0;i<imgSize_;i++){
      for(int j=0;j<imgSize_;j++, ++img_ptr){
        *img_ptr = i*dy_+j*dx_;
      }
    }
    img2_ = cv::Mat::zeros(imgSize_,imgSize_,CV_8UC1);
    img_ptr = (uint8_t*) img2_.data;
    for(int i=0;i<imgSize_;i++){
      for(int j=0;j<imgSize_;j++, ++img_ptr){
        if(j<imgSize_/2 & i<imgSize_/2){
          *img_ptr = 0;
        } else {
          *img_ptr = 255;
        }
      }
    }
    pyr1_.computeFromImage(img1_);
    pyr2_.computeFromImage(img2_);
  }
Exemplo n.º 2
0
 FeatureCoordinates FeatureCoordinates::get_patchCorner(const double x, const double y) const{
   FeatureCoordinates temp; // TODO: avoid temp
   get_nor().boxPlus(get_warp_nor()*Eigen::Vector2d(x,y),norTemp_);
   temp.set_nor(norTemp_);
   temp.mpCamera_ = mpCamera_;
   temp.camID_ = camID_;
   return temp;
 }
Exemplo n.º 3
0
 /** \brief Draws the patch borders into an image.
  *
  * @param drawImg     - Image in which the patch borders should be drawn.
  * @param s           - Scaling factor.
  * @param color       - Line color.
  */
 void drawMultilevelPatchBorder(cv::Mat& drawImg,const FeatureCoordinates& c,const float s, const cv::Scalar& color,const FeatureWarping* mpWarp = nullptr) const{
   if(!c.isInFront()) return;
   if(mpWarp == nullptr){
     patches_[0].drawPatchBorder(drawImg,c.get_c(),s*pow(2.0,nLevels-1),color,Eigen::Matrix2f::Identity());
   } else {
     if(mpWarp->com_affineTransform(&c)){
       patches_[0].drawPatchBorder(drawImg,c.get_c(),s*pow(2.0,nLevels-1),color,mpWarp->get_affineTransform(&c));
     }
   }
 }
Exemplo n.º 4
0
 /** \brief Checks if a patch at a specific image location is still within the reference image.
  *
  *   @param img        - Reference Image.
  *   @param c          - Coordinates of the patch in the reference image.
  *   @param withBorder - Check, using either the patch-patchSize of Patch::patch_ (withBorder = false) or the patch-patchSize
  *                       of the expanded patch Patch::patchWithBorder_ (withBorder = true).
  *   @return true, if the patch is completely located within the reference image.
  */
 static bool isPatchInFrame(const cv::Mat& img,const FeatureCoordinates& c,const bool withBorder = false){
   if(c.isInFront() && c.com_warp_c()){
     const int halfpatch_size = patchSize/2+(int)withBorder;
     if(c.isNearIdentityWarping()){
       if(c.get_c().x < halfpatch_size || c.get_c().y < halfpatch_size || c.get_c().x > img.cols-halfpatch_size || c.get_c().y > img.rows-halfpatch_size){
         return false;
       } else {
         return true;
       }
     } else {
       for(int x = 0;x<2;x++){
         for(int y = 0;y<2;y++){
           const float dx = halfpatch_size*(2*x-1);
           const float dy = halfpatch_size*(2*y-1);
           const float wdx = c.get_warp_c()(0,0)*dx + c.get_warp_c()(0,1)*dy;
           const float wdy = c.get_warp_c()(1,0)*dx + c.get_warp_c()(1,1)*dy;
           const float c_x = c.get_c().x + wdx;
           const float c_y = c.get_c().y + wdy;
           if(c_x < 0 || c_y < 0 || c_x > img.cols || c_y > img.rows){
             return false;
           }
         }
       }
       return true;
     }
   } else {
     return false;
   }
 }
Exemplo n.º 5
0
 /** \brief Extracts a multilevel patch from a given image pyramid.
  *
  * @param pyr         - Image pyramid from which the patch data should be extracted.
  * @param l           - Patches are extracted from pyramid level 0 to l.
  * @param mpCoor      - Coordinates of the patch in the reference image (subpixel coordinates possible).
  * @param mpWarp      - Affine warping matrix. If nullptr not warping is considered.
  * @param withBorder  - If true, both, the general patches and the corresponding expanded patches are extracted.
  */
 void extractMultilevelPatchFromImage(const ImagePyramid<nLevels>& pyr,const FeatureCoordinates& c, const int l = nLevels-1,const FeatureWarping* mpWarp = nullptr,const bool withBorder = false){
   for(unsigned int i=0;i<=l;i++){
     pyr.levelTranformCoordinates(c,coorTemp_,0,i);
     isValidPatch_[i] = true;
     if(mpWarp == nullptr){
       patches_[i].extractPatchFromImage(pyr.imgs_[i],coorTemp_.get_c(),Eigen::Matrix2f::Identity(),withBorder);
     } else {
       patches_[i].extractPatchFromImage(pyr.imgs_[i],coorTemp_.get_c(),mpWarp->get_affineTransform(&c),withBorder);
     }
   }
 }
Exemplo n.º 6
0
 /** \brief Transforms pixel coordinates between two pyramid levels.
  *
  * @Note Invalidates camera and bearing vector, since the camera model is not valid for arbitrary image levels.
  * @param cIn        - Input coordinates
  * @param cOut       - Output coordinates
  * @param l1         - Input pyramid level.
  * @param l2         - Output pyramid level.
  * @return the corresponding pixel coordinates on pyramid level l2.
  */
 void levelTranformCoordinates(const FeatureCoordinates& cIn,FeatureCoordinates& cOut,const int l1, const int l2) const{
   assert(l1<n_levels && l2<n_levels && l1>=0 && l2>=0);
   cOut.set_c((centers_[l1]-centers_[l2])*pow(0.5,l2)+cIn.get_c()*pow(0.5,l2-l1));
   if(cIn.mpCamera_ != nullptr){
     if(cIn.com_warp_c()){
       cOut.set_warp_c(cIn.get_warp_c());
     }
   }
   cOut.camID_ = -1;
   cOut.mpCamera_ = nullptr;
 }
Exemplo n.º 7
0
 /** \brief Checks if the MultilevelPatchFeature's patches are fully located within the corresponding images.
  *
  * @param pyr         - Image pyramid, which should be checked to fully contain the patches.
  * @param l           - Maximal pyramid level which should be checked (Note: The maximal level is the critical level.)
  * @param mpCoor      - Coordinates of the patch in the reference image (subpixel coordinates possible).
  * @param mpWarp      - Affine warping matrix. If nullptr not warping is considered.
  * @param withBorder  - If true, the check is executed with the expanded patch dimensions (incorporates the general patch dimensions).
  *                      If false, the check is only executed with the general patch dimensions.
  */
 bool isMultilevelPatchInFrame(const ImagePyramid<nLevels>& pyr,const FeatureCoordinates& c, const int l = nLevels-1,const FeatureWarping* mpWarp = nullptr,const bool withBorder = false) const{
   if(!c.isInFront()) return false;
   pyr.levelTranformCoordinates(c,coorTemp_,0,l);
   if(mpWarp == nullptr){
     return patches_[l].isPatchInFrame(pyr.imgs_[l],coorTemp_.get_c(),Eigen::Matrix2f::Identity(),withBorder);
   } else {
     if(!mpWarp->com_affineTransform(&c)){
       return false;
     }
     return patches_[l].isPatchInFrame(pyr.imgs_[l],coorTemp_.get_c(),mpWarp->get_affineTransform(&c),withBorder);
   }
 }
Exemplo n.º 8
0
// Test align2D_old
TEST_F(MLPTesting, align2D_old) {
  FeatureCoordinates cAligned;
  c_.set_warp_identity();
  c_.set_c(cv::Point2f(imgSize_/2,imgSize_/2));
  mp_.extractMultilevelPatchFromImage(pyr2_,c_,nLevels_-1,true);
  ASSERT_EQ(mpa_.align2D_old(cAligned,pyr2_,mp_,c_,0,nLevels_-1,100,1e-4),true);
  ASSERT_NEAR(cAligned.get_c().x,imgSize_/2,1e-2);
  ASSERT_NEAR(cAligned.get_c().y,imgSize_/2,1e-2);
  c_.set_c(cv::Point2f(imgSize_/2+1,imgSize_/2+1));
  ASSERT_EQ(mpa_.align2D_old(cAligned,pyr2_,mp_,c_,0,nLevels_-1,100,1e-4),true);
  ASSERT_NEAR(cAligned.get_c().x,imgSize_/2,1e-2);
  ASSERT_NEAR(cAligned.get_c().y,imgSize_/2,1e-2);
  ASSERT_EQ(mpa_.align2D_old(cAligned,pyr2_,mp_,c_,0,0,100,1e-4),true);
  ASSERT_NEAR(cAligned.get_c().x,imgSize_/2,1e-2);
  ASSERT_NEAR(cAligned.get_c().y,imgSize_/2,1e-2);
  ASSERT_EQ(mpa_.align2D_old(cAligned,pyr2_,mp_,c_,1,1,100,1e-4),false);

  Eigen::Matrix2f aff;
  aff << cos(M_PI/2.0), -sin(M_PI/2.0), sin(M_PI/2.0), cos(M_PI/2.0);
  c_.set_warp_c(aff);
  c_.set_c(cv::Point2f(imgSize_/2,imgSize_/2));
  mp_.extractMultilevelPatchFromImage(pyr2_,c_,nLevels_-1,true);
  ASSERT_EQ(mpa_.align2D_old(cAligned,pyr2_,mp_,c_,0,nLevels_-1,100,1e-4),true);
  ASSERT_NEAR(cAligned.get_c().x,imgSize_/2,1e-2);
  ASSERT_NEAR(cAligned.get_c().y,imgSize_/2,1e-2);
  c_.set_c(cv::Point2f(imgSize_/2+1,imgSize_/2+1));
  ASSERT_EQ(mpa_.align2D_old(cAligned,pyr2_,mp_,c_,0,nLevels_-1,100,1e-4),true);
  ASSERT_NEAR(cAligned.get_c().x,imgSize_/2,1e-2);
  ASSERT_NEAR(cAligned.get_c().y,imgSize_/2,1e-2);
  ASSERT_EQ(mpa_.align2D_old(cAligned,pyr2_,mp_,c_,0,0,100,1e-4),true);
  ASSERT_NEAR(cAligned.get_c().x,imgSize_/2,1e-2);
  ASSERT_NEAR(cAligned.get_c().y,imgSize_/2,1e-2);
  ASSERT_EQ(mpa_.align2D_old(cAligned,pyr2_,mp_,c_,1,1,100,1e-4),false);
}
Exemplo n.º 9
0
 bool FeatureCoordinates::getDepthFromTriangulation(const FeatureCoordinates& other, const V3D& C2rC2C1, const QPD& qC2C1, FeatureDistance& d){
   const V3D C2v1 = qC2C1.rotate(get_nor().getVec());
   const V3D C2v2 = other.get_nor().getVec();
   const double a = 1.0-pow(C2v1.dot(C2v2),2.0);
   if(a < 1e-3){
     return false;
   }
   const double distance = C2v1.dot((M3D::Identity()-C2v2*C2v2.transpose())*C2rC2C1) / a;
   d.setParameter(fabs(distance));
   return true;
 }
Exemplo n.º 10
0
bool FeatureCoordinates::getDepthFromTriangulation(const FeatureCoordinates& other, const V3D& C2rC2C1, const QPD& qC2C1, FeatureDistance& d) {
    Eigen::Matrix<double,3,2> B;
    B <<  qC2C1.rotate(get_nor().getVec()), other.get_nor().getVec();
    const Eigen::Matrix2d BtB = B.transpose() * B;
    if(BtB.determinant() < 0.000001) { // Projection rays almost parallel.
        return false;
    }
    const Eigen::Vector2d dv = - BtB.inverse() * B.transpose() * C2rC2C1;
    d.setParameter(fabs(dv[0]));
    return true;
}
Exemplo n.º 11
0
 /** \brief Draws the patch borders into an image.
  *
  * @param drawImg     - Image in which the patch borders should be drawn.
  * @param c           - Coordinates of the patch in the reference image.
  * @param s           - Scaling factor.
  * @param color       - Line color.
  */
 void drawPatchBorder(cv::Mat& drawImg,const FeatureCoordinates& c,const float s, const cv::Scalar& color) const{
   const double half_length = s*patchSize/2;
   if(c.isInFront() && c.com_warp_c()){
     cv::Point2f c1 = c.get_patchCorner(half_length,half_length).get_c();
     cv::Point2f c2 = c.get_patchCorner(half_length,-half_length).get_c();
     cv::line(drawImg,c1,c2,color,1);
     c1 = c.get_patchCorner(-half_length,-half_length).get_c();
     cv::line(drawImg,c2,c1,color,1);
     c2 = c.get_patchCorner(-half_length,half_length).get_c();
     cv::line(drawImg,c1,c2,color,1);
     c1 = c.get_patchCorner(half_length,half_length).get_c();
     cv::line(drawImg,c2,c1,color,1);
   }
 }
Exemplo n.º 12
0
  /** \brief Image callback, handling the tracking of MultilevelPatchFeature%s.
   *
   *  The sequence of the callback can be summarized as follows:
   *  1. Extract image from message. Compute the image pyramid from the extracted image.
   *  2. Predict the position of the valid MultilevelPatchFeature%s in the current image,
   *     using the previous 2 image locations of these MultilevelPatchFeature%s.
   *  3. Execute 2D patch alignment at the predicted MultilevelPatchFeature locations.
   *     If successful the matching status of the MultilevelPatchFeature is set to FOUND and its image location is updated.
   *  4. Prune: Check the MultilevelPatchFeature%s in the MultilevelPatchSet for their quality (MultilevelPatchFeature::isGoodFeature()).
   *            If a bad quality of a MultilevelPatchFeature is recognized, it is set to invalid.
   *  5. Get new features and add them to the MultilevelPatchSet, if there are too little valid MultilevelPatchFeature%s
   *     in the MultilevelPatchSet. MultilevelPatchFeature%s which are stated invalid are replaced by new features.
   *
   *  @param img_msg - Image message (ros)
   */
  void imgCallback(const sensor_msgs::ImageConstPtr & img_msg){
    // Get image from msg
    cv_bridge::CvImagePtr cv_ptr;
    try {
      cv_ptr = cv_bridge::toCvCopy(img_msg, sensor_msgs::image_encodings::TYPE_8UC1);
    } catch (cv_bridge::Exception& e) {
      ROS_ERROR("cv_bridge exception: %s", e.what());
      return;
    }
    cv_ptr->image.copyTo(img_);

    // Timing
    static double last_time = 0.0;
    double current_time = img_msg->header.stamp.toSec();

    // Pyramid
    pyr_.computeFromImage(img_,true);

    // Drawing
    cvtColor(img_, draw_image_, CV_GRAY2RGB);
    const int numPatchesPlot = 10;
    draw_patches_ = cv::Mat::zeros(numPatchesPlot*(patchSize_*pow(2,nLevels_-1)+4),3*(patchSize_*pow(2,nLevels_-1)+4),CV_8UC1);

    // Prediction
    cv::Point2f dc;
    for(unsigned int i=0;i<nMax_;i++){
      if(fsm_.isValid_[i]){
        dc = 0.75*(fsm_.features_[i].mpCoordinates_->get_c() - fsm_.features_[i].log_previous_.get_c());
        fsm_.features_[i].log_previous_ = *(fsm_.features_[i].mpCoordinates_);
        fsm_.features_[i].mpCoordinates_->set_c(fsm_.features_[i].mpCoordinates_->get_c() + dc);
        if(!fsm_.features_[i].mpMultilevelPatch_->isMultilevelPatchInFrame(pyr_,*(fsm_.features_[i].mpCoordinates_),nLevels_-1,false)){
          fsm_.features_[i].mpCoordinates_->set_c(fsm_.features_[i].log_previous_.get_c());
        }
        fsm_.features_[i].mpStatistics_->increaseStatistics(current_time);
        for(int j=0;j<nCam_;j++){
          fsm_.features_[i].mpStatistics_->status_[j] = UNKNOWN;
        }
      }
    }

    // Track valid features
    FeatureCoordinates alignedCoordinates;
    const double t1 = (double) cv::getTickCount();
    for(unsigned int i=0;i<nMax_;i++){
      if(fsm_.isValid_[i]){
        fsm_.features_[i].log_prediction_ = *(fsm_.features_[i].mpCoordinates_);
        if(alignment_.align2DComposed(alignedCoordinates,pyr_,*fsm_.features_[i].mpMultilevelPatch_,*fsm_.features_[i].mpCoordinates_,l2,l1,l1)){
          fsm_.features_[i].mpStatistics_->status_[0] = TRACKED;
          fsm_.features_[i].mpCoordinates_->set_c(alignedCoordinates.get_c());
          fsm_.features_[i].log_previous_ = *(fsm_.features_[i].mpCoordinates_);
          fsm_.features_[i].mpCoordinates_->drawPoint(draw_image_,cv::Scalar(0,255,255));
          fsm_.features_[i].mpCoordinates_->drawLine(draw_image_,fsm_.features_[i].log_prediction_,cv::Scalar(0,255,255));
          fsm_.features_[i].mpCoordinates_->drawText(draw_image_,std::to_string(i),cv::Scalar(0,255,255));
          if(i==18){
            for(int j=0;j<nLevels_;j++){
//              std::cout << fsm_.features_[i].mpMultilevelPatch_->isValidPatch_[j] << std::endl;
//              std::cout << fsm_.features_[i].mpMultilevelPatch_->patches_[j].dx_[0] << std::endl;
            }
//            std::cout << alignment_.A_.transpose() << std::endl;
//            std::cout << alignment_.b_.transpose() << std::endl;
          }

        } else {
          fsm_.features_[i].mpStatistics_->status_[0] = FAILED_ALIGNEMENT;
          fsm_.features_[i].mpCoordinates_->drawPoint(draw_image_,cv::Scalar(0,0,255));
          fsm_.features_[i].mpCoordinates_->drawText(draw_image_,std::to_string(fsm_.features_[i].idx_),cv::Scalar(0,0,255));
        }
      }
    }
    const double t2 = (double) cv::getTickCount();
    ROS_INFO_STREAM(" Matching " << fsm_.getValidCount() << " patches (" << (t2-t1)/cv::getTickFrequency()*1000 << " ms)");
    MultilevelPatch<nLevels_,patchSize_> mp;
    for(unsigned int i=0;i<numPatchesPlot;i++){
      if(fsm_.isValid_[i+10]){
        fsm_.features_[i+10].mpMultilevelPatch_->drawMultilevelPatch(draw_patches_,cv::Point2i(2,2+i*(patchSize_*pow(2,nLevels_-1)+4)),1,false);
        if(mp.isMultilevelPatchInFrame(pyr_,fsm_.features_[i+10].log_prediction_,nLevels_-1,false)){
          mp.extractMultilevelPatchFromImage(pyr_,fsm_.features_[i+10].log_prediction_,nLevels_-1,false);
          mp.drawMultilevelPatch(draw_patches_,cv::Point2i(patchSize_*pow(2,nLevels_-1)+6,2+i*(patchSize_*pow(2,nLevels_-1)+4)),1,false);
        }
        if(fsm_.features_[i+10].mpStatistics_->status_[0] == TRACKED
            && mp.isMultilevelPatchInFrame(pyr_,*fsm_.features_[i+10].mpCoordinates_,nLevels_-1,false)){
          mp.extractMultilevelPatchFromImage(pyr_,*fsm_.features_[i+10].mpCoordinates_,nLevels_-1,false);
          mp.drawMultilevelPatch(draw_patches_,cv::Point2i(2*patchSize_*pow(2,nLevels_-1)+10,2+i*(patchSize_*pow(2,nLevels_-1)+4)),1,false);
          cv::rectangle(draw_patches_,cv::Point2i(0,i*(patchSize_*pow(2,nLevels_-1)+4)),cv::Point2i(patchSize_*pow(2,nLevels_-1)+3,(i+1)*(patchSize_*pow(2,nLevels_-1)+4)-1),cv::Scalar(255),2,8,0);
          cv::rectangle(draw_patches_,cv::Point2i(patchSize_*pow(2,nLevels_-1)+4,i*(patchSize_*pow(2,nLevels_-1)+4)),cv::Point2i(2*patchSize_*pow(2,nLevels_-1)+7,(i+1)*(patchSize_*pow(2,nLevels_-1)+4)-1),cv::Scalar(255),2,8,0);
        } else {
          cv::rectangle(draw_patches_,cv::Point2i(0,i*(patchSize_*pow(2,nLevels_-1)+4)),cv::Point2i(patchSize_*pow(2,nLevels_-1)+3,(i+1)*(patchSize_*pow(2,nLevels_-1)+4)-1),cv::Scalar(0),2,8,0);
          cv::rectangle(draw_patches_,cv::Point2i(patchSize_*pow(2,nLevels_-1)+4,i*(patchSize_*pow(2,nLevels_-1)+4)),cv::Point2i(2*patchSize_*pow(2,nLevels_-1)+7,(i+1)*(patchSize_*pow(2,nLevels_-1)+4)-1),cv::Scalar(0),2,8,0);
        }
        cv::putText(draw_patches_,std::to_string(fsm_.features_[i+10].idx_),cv::Point2i(2,2+i*(patchSize_*pow(2,nLevels_-1)+4)+10),cv::FONT_HERSHEY_SIMPLEX, 0.4, cv::Scalar(255));
      }
    }

    // Prune
    // Check the MultilevelPatchFeature%s in the MultilevelPatchSet for their quality (isGoodFeature(...)).
    // If a bad quality of a MultilevelPatchFeature is recognized, it is set to invalid. New MultilevelPatchFeature%s
    // replace the array places of invalid MultilevelPatchFeature%s in the MultilevelPatchSet.
    int prune_count = 0;
    for(unsigned int i=0;i<nMax_;i++){
      if(fsm_.isValid_[i]){
        if(fsm_.features_[i].mpStatistics_->status_[0] == FAILED_ALIGNEMENT){
          fsm_.isValid_[i] = false;
          prune_count++;
        }
      }
    }
    ROS_INFO_STREAM(" Pruned " << prune_count << " features");

    // Extract feature patches
    // Extract new MultilevelPatchFeature%s at the current tracked feature positions.
    // Extracted multilevel patches are aligned with the image axes.
    for(unsigned int i=0;i<nMax_;i++){
      if(fsm_.isValid_[i]){
        if(fsm_.features_[i].mpStatistics_->status_[0] == TRACKED
            && fsm_.features_[i].mpMultilevelPatch_->isMultilevelPatchInFrame(pyr_,*fsm_.features_[i].mpCoordinates_,nLevels_-1,true)){
          fsm_.features_[i].mpMultilevelPatch_->extractMultilevelPatchFromImage(pyr_,*fsm_.features_[i].mpCoordinates_,nLevels_-1,true);
        }
      }
    }

    // Get new features, if there are too little valid MultilevelPatchFeature%s in the MultilevelPatchSet.
    if(fsm_.getValidCount() < min_feature_count_){
      std::vector<FeatureCoordinates> candidates;
      ROS_INFO_STREAM(" Adding keypoints");
      const double t1 = (double) cv::getTickCount();
      for(int l=l1;l<=l2;l++){
        pyr_.detectFastCorners(candidates,l,detectionThreshold);
      }
      const double t2 = (double) cv::getTickCount();
      ROS_INFO_STREAM(" == Detected " << candidates.size() << " on levels " << l1 << "-" << l2 << " (" << (t2-t1)/cv::getTickFrequency()*1000 << " ms)");
//      pruneCandidates(fsm_,candidates,0);
      const double t3 = (double) cv::getTickCount();
//      ROS_INFO_STREAM(" == Selected " << candidates.size() << " candidates (" << (t3-t2)/cv::getTickFrequency()*1000 << " ms)");
      std::unordered_set<unsigned int> newSet = fsm_.addBestCandidates( candidates,pyr_,0,current_time,
                                                                        l1,l2,max_feature_count_,nDetectionBuckets_, scoreDetectionExponent_,
                                                                        penaltyDistance_, zeroDistancePenalty_,true,0.0);
      const double t4 = (double) cv::getTickCount();
      ROS_INFO_STREAM(" == Got " << fsm_.getValidCount() << " after adding " << newSet.size() << " features (" << (t4-t3)/cv::getTickFrequency()*1000 << " ms)");
      for(auto it = newSet.begin();it != newSet.end();++it){
        fsm_.features_[*it].log_previous_ = *(fsm_.features_[*it].mpCoordinates_);
        for(int j=0;j<nCam_;j++){
          fsm_.features_[*it].mpStatistics_->status_[j] = TRACKED;
        }
      }
    }

    cv::imshow("Tracker", draw_image_);
    cv::imshow("Patches", draw_patches_);
    cv::waitKey(30);
    last_time = current_time;
  }
Exemplo n.º 13
0
  /** \brief Extracts a patch from an image.
   *
   *   @param img        - Reference Image.
   *   @param c          - Coordinates of the patch in the reference image (subpixel coordinates possible).
   *   @param withBorder - If false, the patch object is only initialized with the patch data of the general patch (Patch::patch_).
   *                       If true, the patch object is initialized with both, the patch data of the general patch (Patch::patch_)
   *                       and the patch data of the expanded patch (Patch::patchWithBorder_).
   */
  void extractPatchFromImage(const cv::Mat& img,const FeatureCoordinates& c,const bool withBorder = false){
    assert(isPatchInFrame(img,c,withBorder));
    const int halfpatch_size = patchSize/2+(int)withBorder;
    const int refStep = img.step.p[0];

    // Get Pointers
    uint8_t* img_ptr;
    float* patch_ptr;
    if(withBorder){
      patch_ptr = patchWithBorder_;
    } else {
      patch_ptr = patch_;
    }

    if(c.isNearIdentityWarping()){
      const int u_r = floor(c.get_c().x);
      const int v_r = floor(c.get_c().y);

      // compute interpolation weights
      const float subpix_x = c.get_c().x-u_r;
      const float subpix_y = c.get_c().y-v_r;
      const float wTL = (1.0-subpix_x)*(1.0-subpix_y);
      const float wTR = subpix_x * (1.0-subpix_y);
      const float wBL = (1.0-subpix_x)*subpix_y;
      const float wBR = subpix_x * subpix_y;
      for(int y=0; y<2*halfpatch_size; ++y){
        img_ptr = (uint8_t*) img.data + (v_r+y-halfpatch_size)*refStep + u_r-halfpatch_size;
        for(int x=0; x<2*halfpatch_size; ++x, ++img_ptr, ++patch_ptr)
        {
          *patch_ptr = wTL*img_ptr[0];
          if(subpix_x > 0) *patch_ptr += wTR*img_ptr[1];
          if(subpix_y > 0) *patch_ptr += wBL*img_ptr[refStep];
          if(subpix_x > 0 && subpix_y > 0) *patch_ptr += wBR*img_ptr[refStep+1];
        }
      }
    } else {
      for(int y=0; y<2*halfpatch_size; ++y){
        for(int x=0; x<2*halfpatch_size; ++x, ++patch_ptr){
          const float dx = x - halfpatch_size + 0.5;
          const float dy = y - halfpatch_size + 0.5;
          const float wdx = c.get_warp_c()(0,0)*dx + c.get_warp_c()(0,1)*dy;
          const float wdy = c.get_warp_c()(1,0)*dx + c.get_warp_c()(1,1)*dy;
          const float u_pixel = c.get_c().x+wdx - 0.5;
          const float v_pixel = c.get_c().y+wdy - 0.5;
          const int u_r = floor(u_pixel);
          const int v_r = floor(v_pixel);
          const float subpix_x = u_pixel-u_r;
          const float subpix_y = v_pixel-v_r;
          const float wTL = (1.0-subpix_x) * (1.0-subpix_y);
          const float wTR = subpix_x * (1.0-subpix_y);
          const float wBL = (1.0-subpix_x) * subpix_y;
          const float wBR = subpix_x * subpix_y;
          img_ptr = (uint8_t*) img.data + v_r*refStep + u_r;
          *patch_ptr = wTL*img_ptr[0];
          if(subpix_x > 0) *patch_ptr += wTR*img_ptr[1];
          if(subpix_y > 0) *patch_ptr += wBL*img_ptr[refStep];
          if(subpix_x > 0 && subpix_y > 0) *patch_ptr += wBR*img_ptr[refStep+1];
        }
      }
    }
    if(withBorder){
      extractPatchFromPatchWithBorder();
    }
    validGradientParameters_ = false;
  }
Exemplo n.º 14
0
 void FeatureCoordinates::drawLine(cv::Mat& drawImg, const FeatureCoordinates& other, const cv::Scalar& color, int thickness) const{
   cv::line(drawImg,get_c(),other.get_c(),color,thickness);
 }