void QtOpenCVZedDemo::doStereoSBM_OCL( cv::Mat left, cv::Mat right )
{
cv::ocl::StereoBM_OCL sbm;
//sbm.preset = cv::ocl::StereoBM_OCL::BASIC_PRESET;
//sbm.ndisp = 64;
//sbm.winSize = 9;
sbm.preset = mOclBmWidget->getFilterMode();
sbm.ndisp = mOclBmWidget->getDisp();
sbm.winSize = mOclBmWidget->getWinSize();
if( left.channels() > 1 )
cv::cvtColor( left, left, CV_BGR2GRAY );
if( right.channels() > 1 )
cv::cvtColor( right, right, CV_BGR2GRAY );
cv::ocl::oclMat ocl_left;
cv::ocl::oclMat ocl_right;
cv::ocl::oclMat ocl_disp;
ocl_left.upload( left );
ocl_right.upload( right );
sbm( ocl_left, ocl_right, ocl_disp );
ocl_disp.download( mDisparity );
normalize(mDisparity, mDisparity, 0, 255, CV_MINMAX, CV_8U);
}
void Bedroom::
test()
{
// It should allow different threads to sleep on this object until another thread calls wakeup()
for (int j=0;j<2; j++) {
Bedroom::ptr bedroom(new Bedroom);
int snoozes = 10;
SleepyFaceMock sleepyface1(bedroom, snoozes);
SleepyFaceMock sleepyface2(bedroom, snoozes);
sleepyface1.start ();
sleepyface2.start ();
for (int i=snoozes; i>=0; i--) {
EXCEPTION_ASSERT_EQUALS(sleepyface1.wait (1), i>0?false:true);
EXCEPTION_ASSERT_EQUALS(sleepyface2.wait (1), i>0?false:true);
// sleepyface1 and sleepyface2 shoule be sleeping now
EXCEPTION_ASSERT_EQUALS(bedroom->sleepers(), i>0?2:0);
// they should have 'i' times left to snooze
EXCEPTION_ASSERTX(sleepyface1.snooze () == i && sleepyface2.snooze () == i,
(boost::format("sleepyface1=%d, sleepyface2=%d, i=%d")
% sleepyface1.snooze () % sleepyface2.snooze () % i));
// should wake up both
bedroom->wakeup();
}
EXCEPTION_ASSERT(sleepyface1.isFinished ());
EXCEPTION_ASSERT(sleepyface2.isFinished ());
EXCEPTION_ASSERT_EQUALS(bedroom->sleepers(), 0);
}
// It should throw a BedroomClosed exception if someone tries to go to
// sleep when the bedroom is closed.
{
Bedroom b;
b.close ();
EXPECT_EXCEPTION(BedroomClosed, b.getBed().sleep ());
}
// It should just sleep until the given timeout has elapsed
{
Bedroom b;
Timer t;
bool woken_up_by_wakeup_call = b.getBed ().sleep (2);
// The thread was sleeping in its bed for 2 ms. So the elapsed 'Timer'
// time should be more than 2 ms but less than 3 ms.
EXCEPTION_ASSERT_LESS(t.elapsed (), 3e-3);
EXCEPTION_ASSERT_LESS(2e-3, t.elapsed ());
EXCEPTION_ASSERT(!woken_up_by_wakeup_call); // timeout
}
// It should just sleep until the given timeout has elapsed
for (int i=0; i<40; i++)
{
//TaskTimer tt("It should just sleep until the given timeout has elapsed");
Bedroom::ptr bedroom(new Bedroom);
SleepingBeautyMock sbm(bedroom, 1);
EXCEPTION_ASSERT_EQUALS(0,bedroom->sleepers ());
sbm.start ();
// wait for the thread to go to sleep and then timeout
for (int j=0; j<100 && 0==sbm.timeoutCount (); j++)
EXCEPTION_ASSERT( !sbm.wait (1) );
// wakeup
bedroom->wakeup();
// the thread should finish soon
EXCEPTION_ASSERT( sbm.wait (128) );
EXCEPTION_ASSERT_EQUALS(0,bedroom->sleepers ());
}
// It should just sleep until the given timeout has elapsed
for (int i=0; i<40; i++)
{
//TaskTimer tt("It should just sleep until the given timeout has elapsed");
Bedroom::ptr bedroom(new Bedroom);
SleepingBeautyMock sbm(bedroom, 1000);
EXCEPTION_ASSERT_EQUALS(0,bedroom->sleepers ());
sbm.start ();
// wait for the thread to go to sleep
for (int j=0; j<100 && 0==bedroom->sleepers (); j++)
EXCEPTION_ASSERT( !sbm.wait (1) );
EXCEPTION_ASSERT_EQUALS(1,bedroom->sleepers ());
// wakeup
bedroom->wakeup();
// the thread should finish soon
EXCEPTION_ASSERT( sbm.wait (128) );
EXCEPTION_ASSERT_EQUALS(0,bedroom->sleepers ());
// with such a long timeout the number of timeouts should be 0
EXCEPTION_ASSERT_EQUALS(0,sbm.timeoutCount ());
}
}
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_);
}
