OpenCV3.0 RGB-D Odometry 평가 프로그램

Viz를 사용하여 멋지게 표시

    viz::Viz3d myWindow("Point Cloud");
    /// Pose of the widget in camera frame
    cv::Affine3d cloud_pose = cv::Affine3d().translate(cv::Vec3d(0.0f, 0.0f, 0.0f));
    /// Pose of the widget in global frame
    cv::Affine3d cloud_pose_global = cloud_pose;
    cv::Affine3d cam_pose;

            //10フレームごと変換して表示
            Mat rot = Rts[count](Rect(0, 0, 3, 3)).t();
            Mat tvec = Rts[count](Rect(3, 0, 1, 3)).t();
            if (count % 10 == 0){
                int downSamplingNum = 4; //e.g. 4
                Mat image2(image.rows / downSamplingNum, image.cols / downSamplingNum, CV_8UC3);
                resize(image, image2, image2.size(), 0, 0, INTER_LINEAR);
                Mat pCloud(image.rows / downSamplingNum, image.cols / downSamplingNum, CV_64FC3);

                for (int y = 0; y < 480; y += downSamplingNum){
                    for (int x = 0; x < 640; x += downSamplingNum){
                        if (depth.at<float>(y, x) < 8.0 && depth.at<float>(y, x) > 0.4){
                            //RGB-D Dataset
                            Mat pmat(1, 3, CV_64F);
                            pmat.at<double>(0, 2) = (double)depth.at<float>(y, x);
                            pmat.at<double>(0, 0) = (x - cx) * pmat.at<double>(0, 2) / fx;
                            pmat.at<double>(0, 1) = (y - cy) * pmat.at<double>(0, 2) / fy;
                            pmat = (pmat)*rot + tvec;
                            Point3d p(pmat);
                            pCloud.at<Point3d>(y / downSamplingNum, x / downSamplingNum) = p;
                            pmat.release();
                        }
                        else{
                            //RGB-D Dataset
                            pCloud.at<Vec3d>(y / downSamplingNum, x / downSamplingNum) = Vec3d(0.f, 0.f, 0.f);
                        }
                    }
                }

                viz::WCloud wcloud(pCloud, image2);
                string myWCloudName = "CLOUD" + NumToString(count);
                myWindow.showWidget(myWCloudName, wcloud, cloud_pose_global);

cam_pose = cv::Affine3d(rot.t(), tvec);
            viz::WCameraPosition cpw(0.1); // Coordinate axes
            viz::WCameraPosition cpw_frustum(cv::Matx33d(cameraMatrix), /*image,*/ 0.1, viz::Color::white()); // Camera frustum
            string widgetPoseName = "CPW" + NumToString(count);
            string widgetFrustumName = "CPW_FRUSTUM" + NumToString(count);
            myWindow.showWidget(widgetPoseName, cpw, cam_pose);
            myWindow.showWidget(widgetFrustumName, cpw_frustum, cam_pose);
            myWindow.spinOnce(1, true);

실험 환경

실행 준비

결과

관련 기사