Stereo vision Archives - Robotics with ROS

Computing Fundamental Matrix and Drawing Epipolar Lines for Stereo Vision Cameras in OpenCV

Leave a Comment / C++, Computer Vision, Tutorials / admin

Following my other post, you can extract the equation for epipolar lines. Basically choosing one point in one image and using fundamental matrix, we will get a line in the other image:

    /*
    FM_7POINT   7-point algorithm
    FM_8POINT   8-point algorithm
    FM_LMEDS    least-median algorithm. 7-point algorithm is used.
    FM_RANSAC   ANSAC algorithm. It needs at least 15 points. 7-point algorithm is used
    */	

    cv::Mat fundamentalMatrix= cv::findFundamentalMat(imagePointsLeftCamera, imagePointsRightCamera, cv::FM_8POINT);
    std::vector<cv::Vec3d> leftLines, rightLines;
    cv::computeCorrespondEpilines(imagePointsLeftCamera, 1, fundamentalMatrix, rightLines);
    cv::computeCorrespondEpilines(imagePointsRightCamera, 2, fundamentalMatrix, leftLines);
	
    cv::Mat leftImageRGB(leftImage.size(), CV_8UC3);
    cv::cvtColor(leftImage, leftImageRGB, CV_GRAY2RGB);
	
    cv::Mat rightImageRGB(rightImage.size(), CV_8UC3);
    cv::cvtColor(rightImage, rightImageRGB, CV_GRAY2RGB);

    cv::Mat imagePointLeftCameraMatrix=cv::Mat_<double>(3,1);

    for(std::size_t i=0;i<rightLines.size();i=i+1)
    {
        cv::Vec3d l=rightLines.at(i);
	double a=l.val[0];
        double b=l.val[1];
        double c=l.val[2];
        std::cout<<"------------------------a,b,c Using OpenCV (ax+by+c=0)------------------------------"<<std::endl;
        std::cout<< a <<", "<<b <<", "<<c <<std::endl;
        std::cout<<"------------------------calculating a,b,c (ax+by+c=0) ------------------------------"<<std::endl;
        
        imagePointLeftCameraMatrix.at<double>(0,0)=imagePointsLeftCamera[i].x;
        imagePointLeftCameraMatrix.at<double>(1,0)=imagePointsLeftCamera[i].y;
        imagePointLeftCameraMatrix.at<double>(2,0)=1;
        cv::Mat rightLineMatrix=fundamentalMatrix*imagePointLeftCameraMatrix;
        
        std::cout<< rightLineMatrix.at<double>(0,0) <<", "<<rightLineMatrix.at<double>(0,1) <<", "<<rightLineMatrix.at<double>(0,2) <<std::endl;

        /////////////////////////////////drawing the line on the image/////////////////////////////////
        /*ax+by+c=0*/
        double x0,y0,x1,y1;
        x0=0;
        y0=(-c-a*x0)/b;
	x1=rightImageRGB.cols;
        y1=(-c-a*x1)/b;

	std::cout<<"error: "<< a*imagePointsRightCamera.at(i).x+ b*imagePointsRightCamera.at(i).y +c<<std::endl;
	cv::line(rightImageRGB, cvPoint(x0,y0), cvPoint(x1,y1), cvScalar(0,255,0), 1);
    }

    cv::imwrite("leftImageEpipolarLine.jpg",leftImageRGB);

FM_7POINT 7-point algorithm

FM_8POINT 8-point algorithm

FM_LMEDS least-median algorithm. 7-point algorithm is used.

FM_RANSAC ANSAC algorithm. It needs at least 15 points. 7-point algorithm is used

cv::Mat fundamentalMatrix= cv::findFundamentalMat(imagePointsLeftCamera, imagePointsRightCamera, cv::FM_8POINT);

std::vector<cv::Vec3d> leftLines, rightLines;

cv::computeCorrespondEpilines(imagePointsLeftCamera, 1, fundamentalMatrix, rightLines);

cv::computeCorrespondEpilines(imagePointsRightCamera, 2, fundamentalMatrix, leftLines);

cv::Mat leftImageRGB(leftImage.size(), CV_8UC3);

cv::cvtColor(leftImage, leftImageRGB, CV_GRAY2RGB);

cv::Mat rightImageRGB(rightImage.size(), CV_8UC3);

cv::cvtColor(rightImage, rightImageRGB, CV_GRAY2RGB);

cv::Mat imagePointLeftCameraMatrix=cv::Mat_<double>(3,1);

for(std::size_t i=0;i<rightLines.size();i=i+1)

{

cv::Vec3d l=rightLines.at(i);

double a=l.val[0];

double b=l.val[1];

double c=l.val[2];

std::cout<<"------------------------a,b,c Using OpenCV (ax+by+c=0)------------------------------"<<std::endl;

std::cout<< a <<", "<<b <<", "<<c <<std::endl;

std::cout<<"------------------------calculating a,b,c (ax+by+c=0) ------------------------------"<<std::endl;

imagePointLeftCameraMatrix.at<double>(0,0)=imagePointsLeftCamera[i].x;

imagePointLeftCameraMatrix.at<double>(1,0)=imagePointsLeftCamera[i].y;

imagePointLeftCameraMatrix.at<double>(2,0)=1;

cv::Mat rightLineMatrix=fundamentalMatrix*imagePointLeftCameraMatrix;

std::cout<< rightLineMatrix.at<double>(0,0) <<", "<<rightLineMatrix.at<double>(0,1) <<", "<<rightLineMatrix.at<double>(0,2) <<std::endl;

/////////////////////////////////drawing the line on the image/////////////////////////////////

/*ax+by+c=0*/

double x0,y0,x1,y1;

x0=0;

y0=(-c-a*x0)/b;

x1=rightImageRGB.cols;

y1=(-c-a*x1)/b;

std::cout<<"error: "<< a*imagePointsRightCamera.at(i).x+ b*imagePointsRightCamera.at(i).y +c<<std::endl;

cv::line(rightImageRGB, cvPoint(x0,y0), cvPoint(x1,y1), cvScalar(0,255,0), 1);

}

cv::imwrite("leftImageEpipolarLine.jpg",leftImageRGB);

Computing Fundamental Matrix and Drawing Epipolar Lines for Stereo Vision Cameras in OpenCV Read More »

Creating a Simulated Stereo Vision Cameras With OpenCV and C++

Leave a Comment / C++, Computer Vision, Linear Algebra, Tutorials / admin

n this tutorial, I created an ellipsoid in 3D space and create two cameras in right and left and captured the image:

cv::Mat leftCameraRotation,rightCameraRotation;
	double rollLeft, pitchLeft,  yawLeft,rollRight, pitchRight,  yawRight ,txLeft,tyLeft,tzLeft,txRight,tyRight,tzRight;

	cv::Vec3d thetaLeft,thetaRight;

	rollLeft=0 ;
	pitchLeft=+M_PI/10;
	yawLeft=0;
	
	thetaLeft[0]=rollLeft;
	thetaLeft[1]=pitchLeft;
	thetaLeft[2]=yawLeft;
	
	rollRight=0;
	pitchRight= -M_PI/10;
	yawRight= 0;
		
	thetaRight[0]=rollRight;
	thetaRight[1]=pitchRight;
	thetaRight[2]=yawRight;
	
	txLeft=-1;
	tyLeft=0.0;
	tzLeft=-4.0;
		
	txRight=1.0;
	tyRight=0.0;
	tzRight=-4.0;
	
	leftCameraRotation =eulerAnglesToRotationMatrix(thetaLeft);
	rightCameraRotation =eulerAnglesToRotationMatrix(thetaRight);
		
	cv::Mat leftCameraTranslation = (cv::Mat_<double>(3,1) <<txLeft,tyLeft,tzLeft);
	cv::Mat rightCameraTranslation = (cv::Mat_<double>(3,1) <<txRight,tyRight,tzRight);
	
	std::vector<cv::Point3d> objectPointsInWorldCoordinate;
	double X,Y,Z,radius;
	

////////////////////////////////////////creating ellipsoid in the world coordinate///////////////////////	
	double phiStepSize,thetaStepSize;
	phiStepSize=0.7;
	thetaStepSize=0.6;
	double a,b,c;
	a=2;
	b=3;
	c=1.6;
	for(double phi=-M_PI;phi<M_PI;phi=phi+phiStepSize)
	{
		for(double theta=-M_PI/2;theta<M_PI/2;theta=theta+thetaStepSize)
		{
			X=a*cos(theta)*cos(phi);
			Y=b*cos(theta)*sin(phi);
			Z=c*sin(theta);
			objectPointsInWorldCoordinate.push_back(cv::Point3d(X, Y, Z));
		}
	}

	int numberOfPixelInHeight,numberOfPixelInWidth;
	double heightOfSensor, widthOfSensor;
	double focalLength=2.0;
	double mx, my, U0, V0;
	numberOfPixelInHeight=600;
	numberOfPixelInWidth=600;
	
	heightOfSensor=10;
	widthOfSensor=10;
	
	my=(numberOfPixelInHeight)/heightOfSensor ;
	U0=(numberOfPixelInHeight)/2 ;

	mx=(numberOfPixelInWidth)/widthOfSensor; 
	V0=(numberOfPixelInWidth)/2;

	cv::Mat K = (cv::Mat_<double>(3,3) <<
	focalLength*mx, 0, V0,
	0,focalLength*my,U0,
	0,0,1);
	
	std::vector<cv::Point2d> imagePointsLeftCamera,imagePointsRightCamera;
	cv::projectPoints(objectPointsInWorldCoordinate, leftCameraRotation, leftCameraTranslation, K, cv::noArray(), imagePointsLeftCamera);
	cv::projectPoints(objectPointsInWorldCoordinate, rightCameraRotation, rightCameraTranslation, K, cv::noArray(), imagePointsRightCamera);

////////////////////////////////////////////////storing images from right and left camera//////////////////////////////////////////////

	std::string fileName;
	cv::Mat rightImage,leftImage;
	int U,V;
	leftImage=cv::Mat::zeros(numberOfPixelInHeight,numberOfPixelInWidth,CV_8UC1);

	for(std::size_t i=0;i<imagePointsLeftCamera.size();i++)
	{
		V=int(imagePointsLeftCamera.at(i).x);
		U=int(imagePointsLeftCamera.at(i).y);
		leftImage.at<char>(U,V)=255;
	}

	fileName=std::string("imagePointsLeftCamera")+std::to_string(focalLength)+ std::string("_.jpg");
	cv::imwrite(fileName,leftImage);

	rightImage=cv::Mat::zeros(numberOfPixelInHeight,numberOfPixelInWidth,CV_8UC1);
	for(std::size_t i=0;i<imagePointsRightCamera.size();i++)
	{
		V=int(imagePointsRightCamera.at(i).x);
		U=int(imagePointsRightCamera.at(i).y);
		rightImage.at<char>(U,V)=255;
	}

	fileName=std::string("imagePointsRightCamera")+std::to_string(focalLength)+ std::string("_.jpg");
	cv::imwrite(fileName,rightImage);

100

101

102

103

104

105

106

107

108

109

110

cv::Mat leftCameraRotation,rightCameraRotation;

double rollLeft, pitchLeft, yawLeft,rollRight, pitchRight, yawRight ,txLeft,tyLeft,tzLeft,txRight,tyRight,tzRight;

cv::Vec3d thetaLeft,thetaRight;

rollLeft=0 ;

pitchLeft=+M_PI/10;

yawLeft=0;

thetaLeft[0]=rollLeft;

thetaLeft[1]=pitchLeft;

thetaLeft[2]=yawLeft;

rollRight=0;

pitchRight= -M_PI/10;

yawRight= 0;

thetaRight[0]=rollRight;

thetaRight[1]=pitchRight;

thetaRight[2]=yawRight;

txLeft=-1;

tyLeft=0.0;

tzLeft=-4.0;

txRight=1.0;

tyRight=0.0;

tzRight=-4.0;

leftCameraRotation =eulerAnglesToRotationMatrix(thetaLeft);

rightCameraRotation =eulerAnglesToRotationMatrix(thetaRight);

cv::Mat leftCameraTranslation = (cv::Mat_<double>(3,1) <<txLeft,tyLeft,tzLeft);

cv::Mat rightCameraTranslation = (cv::Mat_<double>(3,1) <<txRight,tyRight,tzRight);

std::vector<cv::Point3d> objectPointsInWorldCoordinate;

double X,Y,Z,radius;

////////////////////////////////////////creating ellipsoid in the world coordinate///////////////////////

double phiStepSize,thetaStepSize;

phiStepSize=0.7;

thetaStepSize=0.6;

double a,b,c;

a=2;

b=3;

c=1.6;

for(double phi=-M_PI;phi<M_PI;phi=phi+phiStepSize)

{

for(double theta=-M_PI/2;theta<M_PI/2;theta=theta+thetaStepSize)

{

X=a*cos(theta)*cos(phi);

Y=b*cos(theta)*sin(phi);

Z=c*sin(theta);

objectPointsInWorldCoordinate.push_back(cv::Point3d(X, Y, Z));

}

int numberOfPixelInHeight,numberOfPixelInWidth;

double heightOfSensor, widthOfSensor;

double focalLength=2.0;

double mx, my, U0, V0;

numberOfPixelInHeight=600;

numberOfPixelInWidth=600;

heightOfSensor=10;

widthOfSensor=10;

my=(numberOfPixelInHeight)/heightOfSensor ;

U0=(numberOfPixelInHeight)/2 ;

mx=(numberOfPixelInWidth)/widthOfSensor;

V0=(numberOfPixelInWidth)/2;

cv::Mat K = (cv::Mat_<double>(3,3) <<

focalLength*mx, 0, V0,

0,focalLength*my,U0,

0,0,1);

std::vector<cv::Point2d> imagePointsLeftCamera,imagePointsRightCamera;

cv::projectPoints(objectPointsInWorldCoordinate, leftCameraRotation, leftCameraTranslation, K, cv::noArray(), imagePointsLeftCamera);

cv::projectPoints(objectPointsInWorldCoordinate, rightCameraRotation, rightCameraTranslation, K, cv::noArray(), imagePointsRightCamera);

////////////////////////////////////////////////storing images from right and left camera//////////////////////////////////////////////

std::string fileName;

cv::Mat rightImage,leftImage;

int U,V;

leftImage=cv::Mat::zeros(numberOfPixelInHeight,numberOfPixelInWidth,CV_8UC1);

for(std::size_t i=0;i<imagePointsLeftCamera.size();i++)

{

V=int(imagePointsLeftCamera.at(i).x);

U=int(imagePointsLeftCamera.at(i).y);

leftImage.at<char>(U,V)=255;

}

fileName=std::string("imagePointsLeftCamera")+std::to_string(focalLength)+ std::string("_.jpg");

cv::imwrite(fileName,leftImage);

rightImage=cv::Mat::zeros(numberOfPixelInHeight,numberOfPixelInWidth,CV_8UC1);

for(std::size_t i=0;i<imagePointsRightCamera.size();i++)

{

V=int(imagePointsRightCamera.at(i).x);

U=int(imagePointsRightCamera.at(i).y);

rightImage.at<char>(U,V)=255;

}

fileName=std::string("imagePointsRightCamera")+std::to_string(focalLength)+ std::string("_.jpg");

cv::imwrite(fileName,rightImage);

Creating a Simulated Stereo Vision Cameras With OpenCV and C++ Read More »

Geometry of Stereo Vision explained

Leave a Comment / Computer Vision, Tutorials / admin

In this video, I explain the geometry of stereo Vision. Please have look at my other tutorial about finding the essential and fundamental matrix.

Geometry of Stereo Vision explained Read More »

RGBD PCL point cloud from Stereo vision with ROS and OpenCV

Leave a Comment / Computer Vision, Image Processing, ROS, Tutorials / admin

In my other tutorial, I showed you how to calibrate you stereo camera. After Calibration, we can get disparity map and RGBD PCL point cloud from our stereo camera cool huh 🙂 1)Save the following text under “stereo_usb_cam_stream_publisher.launch”

<launch>

<arg name="video_device_right" default="/dev/video1" />
<arg name="video_device_left" default="/dev/video2" />

<arg name="image_width" default="640" />
<arg name="image_height" default="480" />

<arg name="right_image_namespace" default="stereo" />
<arg name="left_image_namespace" default="stereo" />

<arg name="right_image_node" default="right" />
<arg name="left_image_node" default="left" />

<arg name="image_topic_name_right" default="image_raw" />
<arg name="camera_info_topic_name_right" default="camera_info" />

<arg name="image_topic_name_left" default="image_raw" />
<arg name="camera_info_topic_name_left" default="camera_info" />



<arg name="left_camera_name" default="left" />
<arg name="right_camera_name" default="right" />

<arg name="left_camera_frame_id" default="left_camera" />
<arg name="right_camera_frame_id" default="right_camera" />

<arg name="framerate" default="30" />

<arg name="camera_info" default="false" />
<arg name="camera_info_path" default="stereo_camera_info" />
	  
<node ns="$(arg right_image_namespace)" name="$(arg right_image_node)" pkg="usb_cam" type="usb_cam_node" output="screen" >
	<param name="video_device" value="$(arg video_device_right)" />
	<param name="image_width" value="$(arg image_width)" />
	<param name="image_height" value="$(arg image_height)"/>
	<param name="pixel_format" value="yuyv" />
	<param name="io_method" value="mmap"/>
	<remap from="/usb_cam/image_raw" to="$(arg image_topic_name_right)" />
	<param name="framerate" value="$(arg framerate)"/> 
	<!-- if camera_info is available, we will use it-->
	<param name="camera_frame_id" value="$(arg right_camera_frame_id)" if="$(arg camera_info)" /> 
	<param name="camera_info_url" value="file://${ROS_HOME}/$(arg camera_info_path)/right.yaml" if="$(arg camera_info)"/>
	<param name="camera_name" value="narrow_stereo/$(arg right_camera_name)" if="$(arg camera_info)"/> 
	<remap from="/usb_cam/camera_info" to="$(arg camera_info_topic_name_right)" if="$(arg camera_info)"/>

</node>

<node ns="$(arg left_image_namespace)" name="$(arg left_image_node)" pkg="usb_cam" type="usb_cam_node" output="screen" >
	<param name="video_device" value="$(arg video_device_left)" />
	<param name="image_width" value="$(arg image_width)" />
	<param name="image_height" value="$(arg image_height)"/>
	<param name="pixel_format" value="yuyv" />
	<param name="io_method" value="mmap"/>
	<param name="framerate" value="$(arg framerate)"/> 
	<remap from="/usb_cam/image_raw" to="$(arg image_topic_name_left)"/>
	<!-- if camera_info is available, we will use it-->
	<param name="camera_frame_id" value="$(arg left_camera_frame_id)" if="$(arg camera_info)"/> 
	<param name="camera_name" value="narrow_stereo/$(arg left_camera_name)" if="$(arg camera_info)"/> 
	<param name="camera_info_url" value="file://${ROS_HOME}/$(arg camera_info_path)/left.yaml" if="$(arg camera_info)"/> 
	<remap from="/usb_cam/camera_info" to="$(arg camera_info_topic_name_left)" if="$(arg camera_info)"/>
</node>
</launch>

</node>

</node>

</launch>

2) Then run the following node to publish both cameras and camera info (calibration matrix)

roslaunch stereo_usb_cam_stream_publisher.launch video_device_right:=/dev/video1 video_device_left:=/dev/video2 image_width:=640 image_height:=480 left_camera_name:=left right_camera_name:=right camera_info:=true camera_info_path:=stereo_camera_info

1	roslaunch stereo_usb_cam_stream_publisher.launch video_device_right:=/dev/video1 video_device_left:=/dev/video2 image_width:=640 image_height:=480 left_camera_name:=left right_camera_name:=right camera_info:=true camera_info_path:=stereo_camera_info

3) Run the

RGBD PCL point cloud from Stereo vision with ROS and OpenCV Read More »

Cookie	Duration	Description
cookielawinfo-checbox-analytics	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Analytics".
cookielawinfo-checbox-functional	11 months	The cookie is set by GDPR cookie consent to record the user consent for the cookies in the category "Functional".
cookielawinfo-checbox-others	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Other.
cookielawinfo-checkbox-necessary	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookies is used to store the user consent for the cookies in the category "Necessary".
cookielawinfo-checkbox-performance	11 months	This cookie is set by GDPR Cookie Consent plugin. The cookie is used to store the user consent for the cookies in the category "Performance".
viewed_cookie_policy	11 months	The cookie is set by the GDPR Cookie Consent plugin and is used to store whether or not user has consented to the use of cookies. It does not store any personal data.