Как использовать функции контуров и углов для получения 2D-соответствий для точек 3D-модели CAD в решенииPnP? - PullRequest
Новая
       99

Как использовать функции контуров и углов для получения 2D-соответствий для точек 3D-модели CAD в решенииPnP?

0 голосов
/ 09 июля 2020

У меня есть код для извлечения 2D-координат из предварительных знаний, например, с использованием углов Харриса и контура объекта. Я использую эти функции, потому что объекты без текстур, поэтому ORB, SIFT или SURF работать не будут. Моя цель - получить 2D-соответствия для точек моей 3D-модели CAD и использовать их в resolvePnPRansa c для отслеживания объекта и получения 6D-позы в режиме реального времени.

Я создал код для Harris Corners Detection и Contours Обнаружение тоже. Они находятся в двух разных исходных кодах C ++. Вот код C ++ для обнаружения Harris Corners 

class CornerDetection
{
public:
    CornerDetection(ros::NodeHandle nh_);
    ~CornerDetection();
    
protected:
    void myShiTomasi_function(cv::Mat img, cv::Mat img_gray, cv::Mat myShiTomasi_dst);
    void myHarris_function(cv::Mat img, cv::Mat img_gray);
    void imageCB(const sensor_msgs::ImageConstPtr& msg);
    
    image_transport::ImageTransport _imageTransport;
    image_transport::Subscriber image_sub;

    int myShiTomasi_qualityLevel = 50;
    int myHarris_qualityLevel = 50;
    int max_qualityLevel = 100;
    int blockSize_shi = 3;
    int blockSize_harris = 3;
    int apertureSize = 3;
    cv::Mat myHarris_copy, myShiTomasi_copy, Mc;
    double myHarris_minVal, myHarris_maxVal, myShiTomasi_minVal, myShiTomasi_maxVal;
};

    const std::string shiTomasi_win = "My ShiTomasi window";
    const std::string harris_win = "My Harris window";
    cv::RNG rng(12345);

CornerDetection::CornerDetection(ros::NodeHandle nh_): _imageTransport(nh_)
{
    image_sub = _imageTransport.subscribe("/zed/zed_node/right_raw/image_raw_color", 1, &CornerDetection::imageCB, this, image_transport::TransportHints("compressed"));
    
    cv::namedWindow(shiTomasi_win, CV_WINDOW_FREERATIO);
    cv::createTrackbar( " Quality Level:", shiTomasi_win, &myShiTomasi_qualityLevel, max_qualityLevel);
    cv::createTrackbar( " Block Size:", shiTomasi_win, &blockSize_shi, 25);
    cv::namedWindow(harris_win, CV_WINDOW_FREERATIO);
    cv::createTrackbar( " Quality Level:", harris_win, &myHarris_qualityLevel, max_qualityLevel);
    cv::createTrackbar( " Block Size:", harris_win, &blockSize_harris, 25);
}

CornerDetection::~CornerDetection()
{
    cv::destroyAllWindows();
}

void CornerDetection::imageCB(const sensor_msgs::ImageConstPtr& msg)
{
    if(blockSize_harris == 0)
        blockSize_harris = 1;
    if(blockSize_shi == 0)
        blockSize_shi = 1;

    cv::Mat img, img_gray, myHarris_dst, myShiTomasi_dst;
    cv_bridge::CvImagePtr cvPtr;

    try
    {
        cvPtr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
    }
    catch (cv_bridge::Exception& e) 
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    cvPtr->image.copyTo(img);
    cv::cvtColor(img, img_gray, cv::COLOR_BGR2GRAY);

  myHarris_dst = cv::Mat::zeros( img_gray.size(), CV_32FC(6) );
  Mc = cv::Mat::zeros( img_gray.size(), CV_32FC1 );

  cv::cornerEigenValsAndVecs( img_gray, myHarris_dst, blockSize_harris, apertureSize, cv::BORDER_DEFAULT );

  for( int j = 0; j < img_gray.rows; j++ )
        for( int i = 0; i < img_gray.cols; i++ )
         {
            float lambda_1 = myHarris_dst.at<cv::Vec6f>(j, i)[0];
            float lambda_2 = myHarris_dst.at<cv::Vec6f>(j, i)[1];
            Mc.at<float>(j,i) = lambda_1*lambda_2 - 0.04f*pow( ( lambda_1 + lambda_2 ), 2 );
         }

  cv::minMaxLoc( Mc, &myHarris_minVal, &myHarris_maxVal, 0, 0, cv::Mat() );

  this->myHarris_function(img, img_gray);

  myShiTomasi_dst = cv::Mat::zeros( img_gray.size(), CV_32FC1 );
  cv::cornerMinEigenVal( img_gray, myShiTomasi_dst, blockSize_shi, apertureSize, cv::BORDER_DEFAULT );
  cv::minMaxLoc( myShiTomasi_dst, &myShiTomasi_minVal, &myShiTomasi_maxVal, 0, 0, cv::Mat() );

  this->myShiTomasi_function(img, img_gray, myShiTomasi_dst);

  cv::waitKey(2);
}


void CornerDetection::myHarris_function(cv::Mat img, cv::Mat img_gray)
{
    myHarris_copy = img.clone();

    if( myHarris_qualityLevel < 1 )
        myHarris_qualityLevel = 1;

    for( int j = 0; j < img_gray.rows; j++ )
        for( int i = 0; i < img_gray.cols; i++ )
            if( Mc.at<float>(j,i) > myHarris_minVal + ( myHarris_maxVal - myHarris_minVal )*myHarris_qualityLevel/max_qualityLevel )
                cv::circle( myHarris_copy, cv::Point(i,j), 4, cv::Scalar( rng.uniform(0,255), rng.uniform(0,255), rng.uniform(0,255) ), -1, 8, 0 );

    cv::imshow( harris_win, myHarris_copy );
}

void CornerDetection::myShiTomasi_function(cv::Mat img, cv::Mat img_gray, cv::Mat myShiTomasi_dst)
{
    myShiTomasi_copy = img.clone();

    if( myShiTomasi_qualityLevel < 1 )
        myShiTomasi_qualityLevel = 1;

    for( int j = 0; j < img_gray.rows; j++ )
        for( int i = 0; i < img_gray.cols; i++ )
            if( myShiTomasi_dst.at<float>(j,i) > myShiTomasi_minVal + ( myShiTomasi_maxVal - myShiTomasi_minVal )*myShiTomasi_qualityLevel/max_qualityLevel )
                cv::circle( myShiTomasi_copy, cv::Point(i,j), 4, cv::Scalar( rng.uniform(0,255), rng.uniform(0,255), rng.uniform(0,255) ), -1, 8, 0 );
        
    cv::imshow( shiTomasi_win, myShiTomasi_copy );
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "FindKeypoints");
    ros::NodeHandle nh;
    CornerDetection cd(nh);
    ros::spin();
}

А вот функция C ++ для обнаружения Countrours 

void TrackSequential::ContourDetection(cv::Mat thresh_in, cv::Mat &output_)
{
    cv::Mat temp;
    cv::Rect objectBoundingRectangle = cv::Rect(0,0,0,0);
    thresh_in.copyTo(temp);
    std::vector<std::vector<cv::Point> > contours;
    std::vector<cv::Vec4i> hierarchy;
    cv::findContours(temp, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
    if(contours.size()>0)
    {
        std::vector<std::vector<cv::Point> > largest_contour;
        largest_contour.push_back(contours.at(contours.size()-1));
        objectBoundingRectangle = cv::boundingRect(largest_contour.at(0));
        int x = objectBoundingRectangle.x+objectBoundingRectangle.width/2;
        int y = objectBoundingRectangle.y+objectBoundingRectangle.height/2;
        cv::circle(output_,cv::Point(x,y),10,cv::Scalar(0,255,0),2);
    }
}

И у меня есть 3D-модель CAD объекта, который я люблю отслеживать, и оцените позу 6D. Мой вопрос: как использовать обнаруженные 2D-точки (углы и контуры) с моей камерой в solvePnPRansac или solvePnP, чтобы отслеживать объект и получать позу 6D в реальном времени?

Спасибо

...