Question Utiliser cv :: rgbd :: Odometry :: compute


J'utilise C ++ et OpenCV avec une combinaison de ROS. J'utilise des images en direct de mon appareil photo (Intel realsense R200). Je reçois des images en profondeur et RVB de mon appareil photo. Dans mon code c ++, je souhaite utiliser ces images pour obtenir des données odométriques et en faire une trajectoire.

J'essaie d'utiliser la fonction "cv :: rgbd :: Odometry :: compute" pour l'odométrie, mais j'obtiens toujours la valeur false (la valeur "isSuccess" dans le code est toujours 0). Mais je ne sais pas quelle partie je fais mal.

Je lis mes images depuis la caméra en utilisant ROS puis dans la fonction Callback, je convertis d'abord toutes les images en niveaux de gris, puis j'utilise la fonction Surf pour détecter les fonctions. Ensuite, je veux utiliser "compute" pour obtenir la transformation entre l'image actuelle et l'image précédente.

Si j'ai bien compris, "Rt" et "inintRt" sont les résultats de la fonction, il suffit donc de les structurer avec la bonne taille.

Quelqu'un peut-il voir le problème? Est-ce que je manque quelque chose?

boost::shared_ptr<rgbd::Odometry> odom;

Mat Rt = Mat(4,4, CV_64FC1);
Mat initRt = Mat(4,4, CV_64FC1);

Mat prevFtrM; //mask Matrix of previous image
Mat currFtrM; //mask Matrix of current image
Mat tempFtrM;

Mat imgprev;// previous depth image
Mat imgcurr;// current depth image

Mat imgprevC;// previous colored image
Mat imgcurrC;// current colored image


void Surf(Mat img) // detect features of the img and fill currFtrM
{
    int minHessian = 400;
    Ptr<SURF> detector = SURF::create( minHessian );
    vector<KeyPoint> keypoints_1;

    currFtrM = Mat::zeros(img.size(), CV_8U);  // type of mask is CV_8U
    Mat roi(currFtrM, cv::Rect(0,0,img.size().width,img.size().height));
    roi = Scalar(255, 255, 255);

    detector->detect( img, keypoints_1, currFtrM );

    Mat img_keypoints_1;
    drawKeypoints( img, keypoints_1, img_keypoints_1, Scalar::all(-1), DrawMatchesFlags::DEFAULT );
    //-- Show detected (drawn) keypoints
    imshow("Keypoints 1", img_keypoints_1 );
}


void Callback(const sensor_msgs::ImageConstPtr& clr, const sensor_msgs::ImageConstPtr& dpt)
{

    if(!imgcurr.data || !imgcurrC.data) // first frame
    {
        // depth image
        imgcurr = cv_bridge::toCvShare(dpt, sensor_msgs::image_encodings::TYPE_32FC1)->image;

        // colored image
        imgcurrC = cv_bridge::toCvShare(clr, "bgr8")->image;
        cvtColor(imgcurrC, imgcurrC, COLOR_BGR2GRAY);

        //find features in the image
        Surf(imgcurrC);
        prevFtrM = currFtrM;

        //scale color image to size of depth image
        resize(imgcurrC,imgcurrC, imgcurr.size());

        return;
    }

    odom = boost::make_shared<rgbd::RgbdOdometry>(imgcurrC, Odometry::DEFAULT_MIN_DEPTH(), Odometry::DEFAULT_MAX_DEPTH(),               Odometry::DEFAULT_MAX_DEPTH_DIFF(), std::vector< int >(), std::vector< float >(),               Odometry::DEFAULT_MAX_POINTS_PART(), Odometry::RIGID_BODY_MOTION);



    // depth image
    imgprev = imgcurr;
    imgcurr = cv_bridge::toCvShare(dpt, sensor_msgs::image_encodings::TYPE_32FC1)->image;

    // colored image
    imgprevC = imgcurrC;
    imgcurrC = cv_bridge::toCvShare(clr, "bgr8")->image;
    cvtColor(imgcurrC, imgcurrC, COLOR_BGR2GRAY);

    //scale color image to size of depth image
    resize(imgcurrC,imgcurrC, imgcurr.size());
    cv::imshow("Color resized", imgcurrC);

    tempFtrM = currFtrM;
    //detect new features in imgcurrC and save in a vector<Point2f>
    Surf( imgcurrC);

    prevFtrM = tempFtrM;

    //set camera matrix to identity matrix
    float vals[] = {619.137635, 0., 304.793791, 0., 625.407449, 223.984030, 0., 0., 1.};

    const Mat cameraMatrix = Mat(3, 3, CV_32FC1, vals);
    odom->setCameraMatrix(cameraMatrix);


    bool isSuccess = odom->compute( imgprevC, imgprev, prevFtrM,  imgcurrC, imgcurr, currFtrM, Rt, initRt );

    if(isSuccess)
        cout << "isSuccess   " << isSuccess << endl;

}

Mettre à jour: J'ai calibré ma caméra et remplacé la matrice de la caméra par des valeurs réelles.


10
2018-01-11 12:44


origine


Réponses: