ROS Archives - Robotics with ROS

How to use image_geometry and camera_info_manager in ROS

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

camera_info_publisher.cpp:

#include <camera_info_manager/camera_info_manager.h>
int main(int argc, char** argv)
{
    ros::init(argc, argv, "camera_info_publisher");
    ros::NodeHandle nh;

    const std::string cname="prosilica";
    const std::string url="file://${ROS_HOME}/camera_info/prosilica.yaml";
    ros::Publisher pubCameraInfo = nh.advertise<sensor_msgs::CameraInfo>("camera/camera_info", 1);
    camera_info_manager::CameraInfoManager cinfo(nh,cname,url);
    sensor_msgs::CameraInfo msgCameraInfo;
    ros::Rate loop_rate(5);
    while (nh.ok())
    {
        msgCameraInfo=cinfo.getCameraInfo();
        pubCameraInfo.publish(msgCameraInfo);
        ros::spinOnce();
        loop_rate.sleep();
    }
}

#include <camera_info_manager/camera_info_manager.h>

int main(int argc, char** argv)

{

ros::init(argc, argv, "camera_info_publisher");

ros::NodeHandle nh;

const std::string cname="prosilica";

const std::string url="file://${ROS_HOME}/camera_info/prosilica.yaml";

ros::Publisher pubCameraInfo = nh.advertise<sensor_msgs::CameraInfo>("camera/camera_info", 1);

camera_info_manager::CameraInfoManager cinfo(nh,cname,url);

sensor_msgs::CameraInfo msgCameraInfo;

ros::Rate loop_rate(5);

while (nh.ok())

{

msgCameraInfo=cinfo.getCameraInfo();

pubCameraInfo.publish(msgCameraInfo);

ros::spinOnce();

loop_rate.sleep();

}

image_geometry_demo.cpp:

#include <ros/ros.h>
#include <image_geometry/pinhole_camera_model.h>

void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& info_msg)
{
    std::cout<<"===============================cameraInfoCallback===============================" <<std::endl;
    image_geometry::PinholeCameraModel cam_model_;
    cam_model_.fromCameraInfo(info_msg);
    // projection_matrix is the matrix you should use if you don't want to use project3dToPixel() and want to use opencv API
    cv::Matx34d projection_matrix=cam_model_.fullProjectionMatrix();
    std::cout<<cam_model_.project3dToPixel(cv::Point3d(-0.1392072,-0.02571392, 2.50376511) )<<std::endl;
}

int main(int argc,char ** argv)
{
    ros::init(argc,argv,"image_geometry_demo",1);
    ros::NodeHandle nh;
    ros::Subscriber sub = nh.subscribe("camera/camera_info", 1000, cameraInfoCallback);
    ros::spin();
}

#include <ros/ros.h>

#include <image_geometry/pinhole_camera_model.h>

void cameraInfoCallback(const sensor_msgs::CameraInfoConstPtr& info_msg)

{

std::cout<<"===============================cameraInfoCallback===============================" <<std::endl;

image_geometry::PinholeCameraModel cam_model_;

cam_model_.fromCameraInfo(info_msg);

// projection_matrix is the matrix you should use if you don't want to use project3dToPixel() and want to use opencv API

cv::Matx34d projection_matrix=cam_model_.fullProjectionMatrix();

std::cout<<cam_model_.project3dToPixel(cv::Point3d(-0.1392072,-0.02571392, 2.50376511) )<<std::endl;

}

int main(int argc,char ** argv)

{

ros::init(argc,argv,"image_geometry_demo",1);

ros::NodeHandle nh;

ros::Subscriber sub = nh.subscribe("camera/camera_info", 1000, cameraInfoCallback);

ros::spin();

}

CMakeLists.txt

find_package(catkin REQUIRED COMPONENTS image_geometry camera_info_manager roscpp)

1	find_package(catkin REQUIRED COMPONENTS image_geometry camera_info_manager roscpp)

How to use image_geometry and camera_info_manager in ROS 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 »

Stereo Camera Calibration with ROS and OpenCV

5 Comments / Computer Vision, Image Processing, ROS, Tutorials / admin

In this tutorial, I’m gonna show you stereo camera calibration with ROS and OpenCV. So you need a pair of cameras, I bought a pair of this USB webcam which is okay for this task. 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.

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

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

3)Now call the calibration node:

Stereo Camera Calibration with ROS and OpenCV Read More »

A GUI ROS-package for cropping pcl pointcloud with dynamic reconfigure

2 Comments / C++, Computer Vision, Image Processing, ROS, Tutorials / admin

This ROS package enables you to crop the scene from Kinect (input topic type: PCL pointcloud). You can even enable fitting a plane to remove the ground from the scene and by adjusting correct parameter you can get the desired object from the scene. code available on my Github.

A GUI ROS-package for cropping pcl pointcloud with dynamic reconfigure Read More »

Octomap explanierend

Leave a Comment / Machine Learning, ROS, Tutorials / admin

In this tutorial, I explain the concept, probabilistic sensor fusion model and the sensor model used in Octomap library. related publication: OctoMap: An Efficient Probabilistic 3D Mapping Framework Based on Octrees 1)Octamap Volumetric Model 2)Probabilistic Sensor Fusion Model 3)Sensor Model for Laser Range Data

Octomap explanierend Read More »

Expectation Maximization algorithm to obtain Gaussian mixture models for ROS

Leave a Comment / C++, Machine Learning, ROS, Tutorials / admin

I found a really good code at GitHub for fitting a Gaussian Mixture Model (GMM) with Expectation Maximization (EM) for ROS. There are so many parameters that you can change. Some of the most important ones are:

// minimum number of gaussians
#define PARAM_NAME_GAUSSIAN_COUNT_MIN    "gaussian_count_min"
#define PARAM_DEFAULT_GAUSSIAN_COUNT_MIN 1

// search will terminate when the gaussian count reaches this
#define PARAM_NAME_GAUSSIAN_COUNT_MAX    "gaussian_count_max"
#define PARAM_DEFAULT_GAUSSIAN_COUNT_MAX 10

// minimum number of gaussians

#define PARAM_NAME_GAUSSIAN_COUNT_MIN "gaussian_count_min"

#define PARAM_DEFAULT_GAUSSIAN_COUNT_MIN 1

// search will terminate when the gaussian count reaches this

#define PARAM_NAME_GAUSSIAN_COUNT_MAX "gaussian_count_max"

#define PARAM_DEFAULT_GAUSSIAN_COUNT_MAX 10

To find the optimal number of components, it uses Bayesian information criterion (BIC). There are other methods to find

Expectation Maximization algorithm to obtain Gaussian mixture models for ROS Read More »

Ackermann steering car robot model with simulation in Gazebo

3 Comments / ROS, Tutorials / admin

Most of the wheeled robots in ROS use move_base to move the robot. move_base geometry model is based on differential drive which basically transforms a velocity command (twist message) into a command for rotating the left and the right wheels at a different speed which enable the car to turn into the right or left or goes straight. But cars

Ackermann steering car robot model with simulation in Gazebo Read More »

Autonomous navigation of two wheels differential drive robot in Gazebo

1 Comment / ROS / admin

Two wheels differential drive robot (with two caster wheels). List of installed sensors: • Velodyne VLP-16. • Velodyne HDL-32E. • Hokuyo Laser scanner. • IMU. • Microsoft Kinect/Asus Xtion Pro. • RGB Camera. You can manually control the robot with Joystick controller for mapping robot environment. Autonomous navigation is possible by setting goal pose.

Autonomous navigation of two wheels differential drive robot in Gazebo Read More »

Converting sensor_msgs::PCLPointCloud2 to sensor_msgs::PointCloud and reverse

Leave a Comment / C++, Computer Vision, Image Processing, ROS / admin

#include <sensor_msgs/point_cloud_conversion.h>

sensor_msgs::PointCloud2 point_cloud2;
sensor_msgs::PointCloud point_cloud;

sensor_msgs::convertPointCloudToPointCloud2(point_cloud, point_cloud2);
sensor_msgs::convertPointCloud2ToPointCloud(point_cloud2, point_cloud);

#include <sensor_msgs/point_cloud_conversion.h>

sensor_msgs::PointCloud2 point_cloud2;

sensor_msgs::PointCloud point_cloud;

sensor_msgs::convertPointCloudToPointCloud2(point_cloud, point_cloud2);

sensor_msgs::convertPointCloud2ToPointCloud(point_cloud2, point_cloud);

Converting sensor_msgs::PCLPointCloud2 to sensor_msgs::PointCloud and reverse Read More »

Converting pcl::PCLPointCloud2 to pcl::PointCloud and reverse

3 Comments / Computer Vision, Image Processing, ROS / admin

#include <pcl/conversions.h>

1	#include <pcl/conversions.h>

pcl::PCLPointCloud2 point_cloud2;
pcl::PointCloud<pcl::PointXYZ> point_cloud;

pcl::fromPCLPointCloud2( point_cloud2, point_cloud);

pcl::toPCLPointCloud2(point_cloud, point_cloud2);

pcl::PCLPointCloud2 point_cloud2;

pcl::PointCloud<pcl::PointXYZ> point_cloud;

pcl::fromPCLPointCloud2( point_cloud2, point_cloud);

pcl::toPCLPointCloud2(point_cloud, point_cloud2);

more about pcl point cloud and conversion

Converting pcl::PCLPointCloud2 to pcl::PointCloud and reverse 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.