TEST_P(DisparityWLSFilterTest, MultiThreadReproducibility) { if (cv::getNumberOfCPUs() == 1) return; double MAX_DIF = 1.0; double MAX_MEAN_DIF = 1.0 / 256.0; int loopsCount = 2; RNG rng(0); DisparityWLSParams params = GetParam(); Size size = get<0>(params); int srcType = get<1>(params); int guideType = get<2>(params); bool use_conf = get<3>(params); bool use_downscale = get<4>(params); Mat left(size, guideType); randu(left, 0, 255); Mat left_disp(size,srcType); int max_disp = (int)(size.width*0.1); randu(left_disp, 0, max_disp-1); Mat right_disp(size,srcType); randu(left_disp, -max_disp+1, 0); Rect ROI(max_disp,0,size.width-max_disp,size.height); if(use_downscale) { resize(left_disp,left_disp,Size(),0.5,0.5); resize(right_disp,right_disp,Size(),0.5,0.5); ROI = Rect(ROI.x/2,ROI.y/2,ROI.width/2,ROI.height/2); } for (int iter = 0; iter <= loopsCount; iter++) { double lambda = rng.uniform(100.0, 10000.0); double sigma = rng.uniform(1.0, 100.0); Ptr<DisparityWLSFilter> wls_filter = createDisparityWLSFilterGeneric(use_conf); wls_filter->setLambda(lambda); wls_filter->setSigmaColor(sigma); cv::setNumThreads(cv::getNumberOfCPUs()); Mat resMultiThread; wls_filter->filter(left_disp,left,resMultiThread,right_disp,ROI); cv::setNumThreads(1); Mat resSingleThread; wls_filter->filter(left_disp,left,resSingleThread,right_disp,ROI); EXPECT_LE(cv::norm(resSingleThread, resMultiThread, NORM_INF), MAX_DIF); EXPECT_LE(cv::norm(resSingleThread, resMultiThread, NORM_L1), MAX_MEAN_DIF*left.total()); } }
void DuoAvoidance::duoMessageCb(const ait_ros_messages::VioSensorMsg::ConstPtr &msg) { ait_ros_messages::VioSensorMsg duo_msg_ = *msg; printf("Received message from Duo3d-Cam. Timestamp: %d.%d\n",duo_msg_.header.stamp.sec,duo_msg_.header.stamp.nsec); //Use cv_bridge to transform images to cv::Mat cv::Mat img_l_=cv_bridge::toCvCopy(duo_msg_.left_image,"mono8")->image; cv::Mat img_r_=cv_bridge::toCvCopy(duo_msg_.right_image,"mono8")->image; cv::Mat left_disp; cv::Mat right_disp; cv::Mat left; cv::Mat right; left = img_l_.clone(); right = img_r_.clone(); cv::Size img_size =img_l_.size(); //Camera Calibration Parameters of left Camera cv::Mat distortion_cam1(1,5,CV_64FC1,cvScalar(0.)); distortion_cam1.at<double>(0,0)=-0.4186; distortion_cam1.at<double>(0,1)=0.2771; distortion_cam1.at<double>(0,4)=-0.1375; cv::Mat intrinsics_cam1(3,3,CV_64FC1,cvScalar(0.)); intrinsics_cam1.at<double>(0,0)=269.181725; //fx Focal Length intrinsics_cam1.at<double>(1,1)=269.9321; //fy Focal Length intrinsics_cam1.at<double>(0,2)=156.3816; //x0 Principal Point intrinsics_cam1.at<double>(1,2)=113.9369; //y0 Principal Point intrinsics_cam1.at<double>(2,2)=1.; //Camera calibration parameters of right Camera cv::Mat distortion_cam2(1,5,CV_64FC1,cvScalar(0.)); distortion_cam2.at<double>(0,0)=-0.4233; distortion_cam2.at<double>(0,1)=0.2967; distortion_cam2.at<double>(0,4)=-0.1663; cv::Mat intrinsics_cam2(3,3,CV_64FC1,cvScalar(0.)); intrinsics_cam2.at<double>(0,0)=270.1185; //fx Focal Length intrinsics_cam2.at<double>(1,1)=270.7983; //fy Focal Length intrinsics_cam2.at<double>(0,2)=163.4106; //x0 Principal Point intrinsics_cam2.at<double>(1,2)=120.1803; //y0 Principal Point intrinsics_cam2.at<double>(2,2)=1.; //Rotation Matrix R_lr cv::Mat R_lr(3,3,CV_64FC1,cvScalar(0.)); R_lr.at<double>(0,0)=1.; R_lr.at<double>(0,1)=0.0018258; R_lr.at<double>(0,2)=0.0028920; R_lr.at<double>(1,0)=-0.0018415;R_lr.at<double>(1,1)=1.; R_lr.at<double>(1,2)=0.005452; R_lr.at<double>(2,0)=0.0028820; R_lr.at<double>(2,1)=-0.0054579;R_lr.at<double>(2,2)=1.; //Translation r_lr cv::Mat r_lr(3,1,CV_64FC1,cvScalar(0.)); r_lr.at<double>(0,0)=30.5341; r_lr.at<double>(1,0)=-0.2507; r_lr.at<double>(2,0)=-1.6489; //Define StereoRectify Outputs cv::Mat R1(3,3,CV_64FC1,cvScalar(0.)); cv::Mat R2(3,3,CV_64FC1,cvScalar(0.)); cv::Mat P1(3,4,CV_64FC1,cvScalar(0.)); cv::Mat P2(3,4,CV_64FC1,cvScalar(0.)); cv::Mat Q(4,4,CV_64FC1,cvScalar(0.)); cv::Rect validRoi[2]; //Calculate the Stereo Rectification cv::stereoRectify( intrinsics_cam1,distortion_cam1, intrinsics_cam2,distortion_cam2, img_size, R_lr.inv(), r_lr, R1, R2, P1, P2, Q, cv::CALIB_ZERO_DISPARITY, 1, img_size, &validRoi[0], &validRoi[1]); //Prepare remapping & remap (undistort) cv::Mat rmap[2][2]; cv::initUndistortRectifyMap(intrinsics_cam1, distortion_cam1, R1, P1, img_size, CV_16SC2, rmap[0][0], rmap[0][1]); cv::initUndistortRectifyMap(intrinsics_cam2, distortion_cam2, R2, P2, img_size, CV_16SC2, rmap[1][0], rmap[1][1]); remap(img_l_,left,rmap[0][0],rmap[0][1],cv::INTER_LINEAR); remap(img_r_,right,rmap[1][0],rmap[1][1],cv::INTER_LINEAR); //crop_img to be rectangular again and resize to initial size cv::Mat cropped_left=left(validRoi[0]); cv::Mat cropped_right=right(validRoi[1]); cv::Mat resized_left; cv::Mat resized_right; cv::resize(cropped_left,resized_left,img_size,0,0,cv::INTER_AREA); cv::resize(cropped_right,resized_right,img_size,0,0,cv::INTER_AREA); //append Image on left side to not get cropped result cv::Mat img_l_app(img_size.height,img_size.width+90,0,cvScalar(0.)); cv::Mat img_r_app(img_size.height,img_size.width+90,0,cvScalar(0.)); img_l_.copyTo(img_l_app(cv::Rect(90,0,img_l_.cols,img_l_.rows))); img_r_.copyTo(img_r_app(cv::Rect(90,0,img_r_.cols,img_r_.rows))); //Calculate disparity int max_disp=64; int wsize=25; cv::StereoBM sbm(cv::StereoBM::BASIC_PRESET,max_disp,wsize); sbm(resized_left,resized_right,left_disp,CV_16S); sbm(img_l_app,img_r_app,left_disp,CV_16S); left_disp=left_disp(cv::Rect(90,0,img_r_.cols,img_r_.rows)); //Alternatively calculation with stereSGBM (Semi Global Block Matching) but 10x more expensive //cv::StereoSGBM sbm; //sbm.SADWindowSize = 15; //sbm.numberOfDisparities = 64; //sbm.preFilterCap = 63; //sbm.minDisparity = -2; //sbm.uniquenessRatio= 30; //sbm.speckleWindowSize = 100; //sbm.speckleRange = 32; //sbm.disp12MaxDiff =1; //sbm.fullDP=false; //sbm.P1=100; //sbm.P2=200; //sbm(resized_left,resized_right,left_disp); //Convert to 8bit left_disp.convertTo(left_disp,0); // //Debug: Republish left image cv_bridge::CvImage cv_out1; cv_out1.image=img_l_app; cv_out1.encoding="mono8"; cv_out1.toImageMsg(test_output1_); test_pub1_.publish(test_output1_); cv_bridge::CvImage cv_out2; cv_out2.image=left_disp; cv_out2.encoding="mono8"; cv_out2.toImageMsg(test_output2_); test_pub2_.publish(test_output2_); }