Question

我正在使用C ++和OpenCV与ROS的组合。我使用来自相机的实时图像（intel realsense R200）。我从相机中获取深度和RGB图像。在我的c ++代码中，我想使用这些图像来获取odometry数据并从中获取轨迹。

我正在尝试使用＆＃34; cv :: rgbd :: Odometry :: compute＆＃34;用于测距的功能，但我总是得到假的返回值（＆＃34; isSuccess＆＃34;代码中的值总是0）。但我不知道我做错了哪一部分。

我使用ROS从相机读取我的图像，然后在回调功能中，首先我将所有图像转换为灰度，然后使用Surf功能检测功能。然后我想使用＆＃34;计算＆＃34;获得当前帧和上一帧之间的转换。

据我了解＆＃34; Rt＆＃34;和＆＃34; inintRt＆＃34;是函数的输出，所以足以用正确的大小来构造它们。

有人能看到问题吗？我错过了什么吗？

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;

}

更新：我校准了相机并用实际值替换了相机矩阵。

Answer 1

有点晚了，但对某人仍然有用。

在我看来，您从计算中丢失了外部校准：在我的实验中，R200在RGB和景深相机之间具有一个平移分量，您无需考虑。此外，从摄影机参数来看，深度和RGB具有不同的内在特性，而色框具有MODIFIED_BROWN_CONRADY镜头失真（但这是最小的），您是不会失真的吗？

很明显，如果您已经完成所有这些步骤并将已注册的RGB和深度保存在文件中，那我就错了。

使用cv :: rgbd :: Odometry :: compute

1 个答案: