Skip to content

GitLab

Commit ddd662f1 authored by Marius's avatar Marius
Browse files
Options

asd

parent b1ffabf9
Expand all Hide whitespace changes
Inline Side-by-side
Showing with 658 additions and 394 deletions
.idea/workspace.xml
View file @ ddd662f1
This diff is collapsed.
include/comon/rosbaghelper.hpp
View file @ ddd662f1
......@@ -5,6 +5,8 @@
#include <pcl/point_types.h>
#include <boost/assert.hpp>
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <opencv2/opencv.hpp>
using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
......@@ -28,3 +30,9 @@ private:
std::unique_ptr<Data> data_;
};
sensor_msgs::ImagePtr createImageMsg(cv::Mat mat);
std::string getImageType(int number);
cv::Mat convert_16SC1_to_8UC1(cv::Mat mat);
src/comon/rosbaghelper.cpp
View file @ ddd662f1
......@@ -3,6 +3,74 @@
#include <rosbag/view.h>
#include <unordered_map>
#include <pcl_ros/point_cloud.h>
#include <cv_bridge/cv_bridge.h>
sensor_msgs::ImagePtr createImageMsg(cv::Mat mat) {
cv_bridge::CvImage img;
img.image = mat;
auto type = mat.type();
std::string encoding;
switch (type) {
case CV_8UC1: encoding = sensor_msgs::image_encodings::MONO8; break;
case CV_8UC3: encoding = sensor_msgs::image_encodings::RGB8; break;
case CV_16UC1: encoding = sensor_msgs::image_encodings::MONO16; break;
}
img.encoding = encoding;
img.header.stamp = ros::Time::now();
return img.toImageMsg();
}
std::string getImageType(int number)
{
// find type
int imgTypeInt = number%8;
std::string imgTypeString;
switch (imgTypeInt)
{
case 0:
imgTypeString = "8U";
break;
case 1:
imgTypeString = "8S";
break;
case 2:
imgTypeString = "16U";
break;
case 3:
imgTypeString = "16S";
break;
case 4:
imgTypeString = "32S";
break;
case 5:
imgTypeString = "32F";
break;
case 6:
imgTypeString = "64F";
break;
default:
break;
}
// find channel
int channel = (number/8) + 1;
std::stringstream type;
type<<"CV_"<<imgTypeString<<"C"<<channel;
return type.str();
}
cv::Mat convert_16SC1_to_8UC1(cv::Mat mat) {
double min, max;
cv::minMaxLoc(mat, &min, &max);
double factor = 256 / max;
cv::Mat1b image8(mat.size());
mat.convertTo(image8, CV_8UC1, factor);
return image8;
}
class BagFileHelper::Data {
public:
......
src/image_processing_node/main.cpp
View file @ ddd662f1
......@@ -10,166 +10,218 @@
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <comon/rosbaghelper.hpp>
#include <algorithm>
ros::Publisher pubDepth8Bit;
ros::Publisher pubDepthSobel;
ros::Publisher pubGrey;
ros::Publisher pubGreyEdges;
ros::Publisher pubDepthEdges;
ros::Publisher pubMedian;
ros::Publisher pubMedianEdges;
ros::Publisher pubH;
ros::Publisher pubS;
ros::Publisher pubV;
ros::Publisher pubR;
ros::Publisher pubG;
ros::Publisher pubB;
ros::Publisher pubHSV;
std::string getImageType(int number)
{
// find type
int imgTypeInt = number%8;
std::string imgTypeString;
switch (imgTypeInt)
{
case 0:
imgTypeString = "8U";
break;
case 1:
imgTypeString = "8S";
break;
case 2:
imgTypeString = "16U";
break;
case 3:
imgTypeString = "16S";
break;
case 4:
imgTypeString = "32S";
break;
case 5:
imgTypeString = "32F";
break;
case 6:
imgTypeString = "64F";
break;
default:
break;
}
// find channel
int channel = (number/8) + 1;
std::stringstream type;
type<<"CV_"<<imgTypeString<<"C"<<channel;
return type.str();
}
image_transport::Publisher pubGrey;
image_transport::Publisher pubGreyEdges;
image_transport::Publisher pubDepthEdges;
image_transport::Publisher pubMedian;
image_transport::Publisher pubH;
image_transport::Publisher pubS;
image_transport::Publisher pubV;
// Less typing
using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
// We are going to publish clouds ... Therefore we need a publisher!
ros::Publisher publisher;
// We are going to receive clouds ... Therefore we need a subscriber!
ros::Subscriber subscriber;
sensor_msgs::ImagePtr createImageMsg(cv::Mat mat) {
cv_bridge::CvImage img;
img.image = mat;
auto type = mat.type();
std::string encoding;
switch (type) {
case CV_8UC1: encoding = sensor_msgs::image_encodings::MONO8; break;
case CV_8UC3: encoding = sensor_msgs::image_encodings::RGB8; break;
case CV_16UC1: encoding = sensor_msgs::image_encodings::MONO16; break;
}
img.encoding = encoding;
img.header.stamp = ros::Time::now();
return img.toImageMsg();
}
void depthCallback(const sensor_msgs::ImageConstPtr imgPtr) {
// Receive an 16 Bit Single Channel Depth Image
cv::Mat depth16Image = cv_bridge::toCvCopy(imgPtr)->image;
//###########################################################################################################
//########################################## Depth 8 Bit ###################################################
//###########################################################################################################
// Calculate the min and max value of the image
double min, max;
cv::minMaxLoc(depth16Image, &min, &max);
void depthCallback(const sensor_msgs::ImageConstPtr imgPtr) {
auto img = cv_bridge::toCvCopy(imgPtr);
ROS_INFO("image received of type %s", getImageType(img->image.type()).c_str());
double factor = 256.0 / max;
// Downsample the 16 Bit depth image to an 8 Bit depth image using 'factor'
cv::Mat depth8Image;
depth16Image.convertTo(depth8Image, CV_8UC1, factor);
double min, max;
cv::minMaxLoc(img->image, &min, &max);
cv::Mat grey;
img->image.convertTo(grey, CV_8UC1, 256.0 / max);
pubDepthEdges.publish(createImageMsg(depth8Image));
cv::minMaxLoc(grey, &min, &max);
ROS_INFO("Min %f Max %f", min, max);
//###########################################################################################################
//########################################## Derivation ###################################################
//###########################################################################################################
cv::Mat depthEdges = grey.clone();
cv::Mat_<std::int16_t> derivationImg(depth8Image.size());
int orderDerivationX = 1;
int orderDerivationY = 0;
int kernelSize = 3;
cv::Sobel(depth8Image, derivationImg, CV_16SC1, orderDerivationX, orderDerivationY, kernelSize);
cv::Canny(grey, depthEdges, 100, 100);
for (int i = 0; i < derivationImg.rows; ++i) {
for (int j = 0; j < derivationImg.cols; ++j) {
auto pixel = derivationImg(i, j);
pixel = std::abs(pixel);
derivationImg(i, j) = pixel;
if (pixel == 0) {
continue;
}
}
}
cv::Mat sobel8Image = convert_16S1_to_8UC1(derivationImg);
auto greyEdgesMsg = createImageMsg(depthEdges);
pubDepthEdges.publish(greyEdgesMsg);
pubDepthSobel.publish(createImageMsg(sobel8Image));
//###########################################################################################################
//########################################## Depth Canny ###################################################
//###########################################################################################################
// Apply the Canny filter and store the results
cv::Mat depthEdges = depth8Image.clone();
int lowerThreshold = 20;
int upperThreshold = 50;
cv::Canny(depth8Image, depthEdges, lowerThreshold, upperThreshold);
// Publish the results
pubDepthEdges.publish(createImageMsg(depthEdges));
}
void colorCallback(const sensor_msgs::ImageConstPtr imgPtr) {
auto img = cv_bridge::toCvCopy(imgPtr);
ROS_INFO("image received of type %s", getImageType(img->image.type()).c_str());
// Receive an 24 Bit (3 * 8 Bit) RGB color image;
auto img = cv_bridge::toCvCopy(imgPtr)->image;
//###########################################################################################################
//########################################## RGB ###########################################################
//###########################################################################################################
// Create a destination image for each channel (RGB)
cv::Mat3b imgR = img.clone();
cv::Mat3b imgG = img.clone();
cv::Mat3b imgB = img.clone();
// Iterate over all pixels of the Red, Green, and Blue image
for (int i = 0; i < img.rows; ++i) {
for (int j = 0; j < img.cols; ++j) {
// Set all color components to zero except the RED one
imgR(i, j)[1] = 0; /* DELETE ME? - Task RGB */
imgR(i, j)[2] = 0; /* DELETE ME? - Task RGB */
// Set all color components to zero except the GREEN one
imgG(i, j)[0] = 0; /* DELETE ME? - Task RGB */
imgG(i, j)[2] = 0; /* DELETE ME? - Task RGB */
// Set all color components to zero except the BLUE one
imgB(i, j)[0] = 0; /* DELETE ME? - Task RGB */
imgB(i, j)[1] = 0; /* DELETE ME? - Task RGB */
}
}
// Publish the images imgR, imgG and imfB
pubR.publish(createImageMsg(imgR));
pubG.publish(createImageMsg(imgG));
pubB.publish(createImageMsg(imgB));
//###########################################################################################################
//########################################## GREY ##########################################################
//###########################################################################################################
// Converts the RGB color image to an 8 bit greyscale image
cv::Mat grey;
cv::cvtColor(img->image, grey, cv::COLOR_RGB2GRAY);
cv::cvtColor(img, grey, cv::COLOR_RGB2GRAY);
pubGrey.publish(createImageMsg(grey));
//###########################################################################################################
//########################################## HSV ###########################################################
//###########################################################################################################
cv::Mat hsv;
cv::cvtColor(img->image, hsv, cv::COLOR_BGR2HSV);
cv::cvtColor(img, hsv, cv::COLOR_RGB2HSV);
// Splits the HSV Image into three separate images. Each of the resulting images has only one channel.
cv::Mat split[3];
cv::split(hsv, split);
// Publish the splitted images
pubH.publish(createImageMsg(split[0]));
pubS.publish(createImageMsg(split[1]));
pubV.publish(createImageMsg(split[2]));
// Clone the splitted HSV channels
cv::Mat1b imgH = split[0].clone(); // Value Range: 0 = 0°, 180 = 360°
cv::Mat1b imgS = split[1].clone(); // Value Range: 0 = 0%, 255 = 100%
cv::Mat1b imgV = split[2].clone(); // Value Range: 0 = 0%, 255 = 100%
// Iterate over all pixels of the splitted channels and modify them
for (int i = 0; i < imgS.rows; ++i) {
for (int j = 0; j < imgS.cols; ++j) {
imgH(i, j) = imgH(i,j);
imgS(i, j) = imgS(i,j);
imgV(i, j) = imgV(i,j);
}
}
cv::Mat greyEdges = grey.clone();
// Merge the modified HSV channels and covert them to an RGB image
cv::Mat mergedHSV;
std::vector<cv::Mat> channels;
channels.push_back(imgH);
channels.push_back(imgS);
channels.push_back(imgV);
merge(channels, mergedHSV);
cv::cvtColor(mergedHSV, mergedHSV, cv::COLOR_HSV2BGR);
// publish the modified HSV image
pubHSV.publish(createImageMsg(mergedHSV));
//###########################################################################################################
//########################################## Median ########################################################
//###########################################################################################################
// Create a destination image and apply the median blur filter to the greyscale image
cv::Mat median = grey.clone();
cv::medianBlur(grey, median, 11);
cv::Canny(median, greyEdges,100, 100);
auto greyEdgesMsg = createImageMsg(greyEdges);
pubGreyEdges.publish(greyEdgesMsg);
auto greyMsg = createImageMsg(grey);
pubGrey.publish(greyMsg);
auto medianMsg = createImageMsg(median);
pubMedian.publish(medianMsg);
int kernelSize = 11;
cv::medianBlur(grey, median, kernelSize);
pubMedian.publish(createImageMsg(median));
//###########################################################################################################
//########################################## Grey Canny ####################################################
//###########################################################################################################
// Apply the canny filter to the blured image
cv::Mat greyEdges = grey.clone();
int lowerThreshold = 20;
int upperThreshold = 50;
cv::Canny(median, greyEdges, lowerThreshold, upperThreshold);
pubGreyEdges.publish(createImageMsg(greyEdges));
}
int main(int argc, char** argv){
// Init ROS stuff
ros::init(argc, argv, "crs_point_processing_node");
auto node = ros::NodeHandle();
image_transport::ImageTransport it(node);
image_transport::Subscriber subDepth = it.subscribe("/kinect2/sd/image_depth", 1, depthCallback);
image_transport::Subscriber subColor = it.subscribe("/kinect2/hd/image_color", 1, colorCallback);
// Subscriber
ros::Subscriber subDepth = node.subscribe("/kinect2/sd/image_depth", 1, depthCallback);
ros::Subscriber subColor = node.subscribe("/kinect2/hd/image_color", 1, colorCallback);
pubGrey = it.advertise("/crs/grey", 1);
pubGreyEdges = it.advertise("/crs/grey_edges", 1);
pubMedian = it.advertise("/crs/grey_median", 1);
// Publisher
pubDepth8Bit = node.advertise<sensor_msgs::Image>("/crs/depth_8_bit", 1);
pubDepthSobel = node.advertise<sensor_msgs::Image>("/crs/depth_sobel", 1);
pubDepthEdges = it.advertise("/crs/depth_edges", 1);
pubGrey = node.advertise<sensor_msgs::Image>("/crs/grey", 1);
pubGreyEdges = node.advertise<sensor_msgs::Image>("/crs/grey_edges", 1);
pubMedian = node.advertise<sensor_msgs::Image>("/crs/grey_median", 1);
pubMedianEdges = node.advertise<sensor_msgs::Image>("/crs/median_edges", 1);
pubDepthEdges = node.advertise<sensor_msgs::Image>("/crs/depth_edges", 1);
pubH = it.advertise("/crs/H", 1);
pubS = it.advertise("/crs/S", 1);
pubV = it.advertise("/crs/V", 1);
pubHSV = node.advertise<sensor_msgs::Image>("/crs/HSV", 1);
pubH = node.advertise<sensor_msgs::Image>("/crs/H", 1);
pubS = node.advertise<sensor_msgs::Image>("/crs/S", 1);
pubV = node.advertise<sensor_msgs::Image>("/crs/V", 1);
ros::spin();
pubR = node.advertise<sensor_msgs::Image>("/crs/R", 1);
pubG = node.advertise<sensor_msgs::Image>("/crs/G", 1);
pubB = node.advertise<sensor_msgs::Image>("/crs/B", 1);
ros::spin();
return 0;
};
src/image_processing_node_student/main.cpp
View file @ ddd662f1
......@@ -7,63 +7,224 @@
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
// Less typing
using CloudRGBA = pcl::PointCloud<pcl::PointXYZRGBA>;
// We are going to publish clouds ... Therefore we need a publisher!
ros::Publisher publisher;
// We are going to receive clouds ... Therefore we need a subscriber!
ros::Subscriber subscriber;
// The callback function. It gets called whenever we receive a new point loucd.
void callback(CloudRGBA::ConstPtr msgPtr) {
// Print some information
ROS_INFO("Received a cloud with %i points", msgPtr->size());
// Init voxelgrid filter and apply
pcl::VoxelGrid<pcl::PointXYZRGBA> sor;
sor.setInputCloud(msgPtr);
sor.setLeafSize(0.01f, 0.01f, 0.01f);
auto filteredPtr = boost::make_shared<CloudRGBA>();
sor.filter(*filteredPtr);
// Set the header of the message. Just some shabby details ...
filteredPtr->header.frame_id = "filtered_frame";
pcl_conversions::toPCL(ros::Time::now(), filteredPtr->header.stamp);
// Publish cloud
publisher.publish(filteredPtr);
// Print some information
ROS_INFO("Published a cloud with %i points", filteredPtr->size());
#include <image_transport/image_transport.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <comon/rosbaghelper.hpp>
#include <algorithm>
ros::Publisher pubDepth8Bit;
ros::Publisher pubDepthSobel;
ros::Publisher pubGrey;
ros::Publisher pubGreyEdges;
ros::Publisher pubDepthEdges;
ros::Publisher pubMedian;
ros::Publisher pubMedianEdges;
ros::Publisher pubH;
ros::Publisher pubS;
ros::Publisher pubV;
ros::Publisher pubR;
ros::Publisher pubG;
ros::Publisher pubB;
ros::Publisher pubHSV;
void depthCallback(const sensor_msgs::ImageConstPtr imgPtr) {
// Receive an 16 Bit Single Channel Depth Image
cv::Mat depth16Image = cv_bridge::toCvCopy(imgPtr)->image;
//###########################################################################################################
//########################################## Depth 8 Bit ###################################################
//###########################################################################################################
// Calculate the min and max value of the image
double min, max;
cv::minMaxLoc(depth16Image, &min, &max);
double factor = 0.0 / max; /* FIX ME - Task Depth 8 Bit*/
// Downsample the 16 Bit depth image to an 8 Bit depth image using 'factor'
cv::Mat depth8Image;
depth16Image.convertTo(depth8Image, CV_8UC1, factor);
pubDepthEdges.publish(createImageMsg(depth8Image));
//###########################################################################################################
//########################################## Derivation ###################################################
//###########################################################################################################
cv::Mat_<std::int16_t> derivationImg(depth8Image.size());
int orderDerivationX = 1; /* TUNE ME - Task Derivation*/
int orderDerivationY = 0; /* TUNE ME - Task Derivation*/
int kernelSize = 3; /* TUNE ME - Task Derivation*/
cv::Sobel(depth8Image, derivationImg, CV_16SC1, orderDerivationX, orderDerivationY, kernelSize);
for (int i = 0; i < derivationImg.rows; ++i) {
for (int j = 0; j < derivationImg.cols; ++j) {
auto pixel = derivationImg(i, j);
pixel = std::abs(pixel);
derivationImg(i, j) = pixel;
if (pixel == 0) {
continue;
}
}
}
cv::Mat sobel8Image = convert_16S1_to_8UC1(derivationImg);
pubDepthSobel.publish(createImageMsg(sobel8Image));
//###########################################################################################################
//########################################## Depth Canny ###################################################
//###########################################################################################################
// Apply the Canny filter and store the results
cv::Mat depthEdges = depth8Image.clone();
int lowerThreshold = 20; /* TUNE ME - Task Depth Canny*/
int upperThreshold = 50; /* TUNE ME - Task Depth Canny*/
cv::Canny(depth8Image, depthEdges, lowerThreshold, upperThreshold);
// Publish the results
pubDepthEdges.publish(createImageMsg(depthEdges));
}
void colorCallback(const sensor_msgs::ImageConstPtr imgPtr) {
// Receive an 24 Bit (3 * 8 Bit) RGB color image;
auto img = cv_bridge::toCvCopy(imgPtr)->image;
//###########################################################################################################
//########################################## RGB ###########################################################
//###########################################################################################################
// Create a destination image for each channel (RGB)
cv::Mat3b imgR = img.clone();
cv::Mat3b imgG = img.clone();
cv::Mat3b imgB = img.clone();
// Iterate over all pixels of the Red, Green, and Blue image
for (int i = 0; i < img.rows; ++i) {
for (int j = 0; j < img.cols; ++j) {
// Set all color components to zero except the RED one
imgR(i, j)[0] = 0; /* DELETE ME? - Task RGB */
imgR(i, j)[1] = 0; /* DELETE ME? - Task RGB */
imgR(i, j)[2] = 0; /* DELETE ME? - Task RGB */
// Set all color components to zero except the GREEN one
imgG(i, j)[0] = 0; /* DELETE ME? - Task RGB */
imgG(i, j)[1] = 0; /* DELETE ME? - Task RGB */
imgG(i, j)[2] = 0; /* DELETE ME? - Task RGB */
// Set all color components to zero except the BLUE one
imgB(i, j)[0] = 0; /* DELETE ME? - Task RGB */
imgB(i, j)[1] = 0; /* DELETE ME? - Task RGB */
imgB(i, j)[2] = 0; /* DELETE ME? - Task RGB */
}
}
// Publish the images imgR, imgG and imfB
pubR.publish(createImageMsg(imgR));
pubG.publish(createImageMsg(imgG));
pubB.publish(createImageMsg(imgB));
//###########################################################################################################
//########################################## GREY ##########################################################
//###########################################################################################################
// Converts the RGB color image to an 8 bit greyscale image
cv::Mat grey;
cv::cvtColor(img, grey, cv::COLOR_RGB2GRAY);
pubGrey.publish(createImageMsg(grey));