- 利用深度相机获取点云二维图像
- 将二维图像通过相机内参转换成点云
- 取局部点云,对点云进行平面拟合(采用的是SVD分解),利用最小特征值对应的特征向量为平面法向量
利用深度相机获取点云二维图像
- 深度相机原理介绍
- 安装深度相机(Kinect V1)驱动
- 借鉴创客智造的博客:链接
- kinect V1的图像校准:利用上面的教程安装驱动,保证可以获取RGB和IR图像,然后按照链接进行标定
- 将二维图像通过相机内参转换成点云
- 深度图和可见光图的对应
- 获取IR中点云的世界坐标(相对于红外相机)
- 将IR点通过IR-RGB相机外参转换转移到可见光相机坐标系下
- 通过可见光相机的内参矩阵进行投影(将IR中的点和可见光中的图像数据进行对应,并且进行存储-接下来称呼这个信息为RGB_with_Depth)
- 具体代码如下
- 通过Kinect V1拍摄两次,获取相机相对位姿变换
- 通过Kinect V1对同一个场景,在不同位置拍摄两张照片
- 通过步骤5计算两个位置下的RGB_with_Depth
- 对两个场景下的RGB图像进行特征点匹配,获取到特征点对应
- 对特征点对分别在各自的RGB_with_Depth里面寻找对应点,获取对应的点的三维信息
- 对上面的三维空间点对进行外参矩阵的拟合求解,获取两个位置的外参矩阵
代码:
#include<ros/ros.h> //ros标准库头文件
#include<iostream> //C++标准输入输出库
#include<math.h>
#include<cv_bridge/cv_bridge.h>
#include<sensor_msgs/image_encodings.h>
#include<image_transport/image_transport.h>
#include <pthread.h>
#include <pcl/point_cloud.h>
#include <pcl_conversions/pcl_conversions.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/integral_image_normal.h>
#include<opencv2/core/core.hpp>
#include<opencv2/highgui/highgui.hpp>
#include<opencv2/imgproc/imgproc.hpp>
#include <iostream>
#include <Eigen/SVD>
#include <Eigen/Dense>
//using Eigen::MatrixXf;
using namespace Eigen;
using namespace Eigen::internal;
using namespace Eigen::Architecture;
using std::cout;
using std::endl;
using std::stringstream;
using std::string;
bool saveCloud(false);
unsigned int fileNum = 1;
#define PI 3.14159265354
float fx = 525.0; // focal length x
float fy = 525.0; // focal length y
float cx = 319.5; // optical center x
float cy = 239.5; // optical center y
float factor = 1000.0;// for the 32-bit float images in the ROS bag files
float angle=0.0;
float angle_wall=0.0;
class RGB_GRAY
{
public:
ros::NodeHandle nh_;
image_transport::ImageTransport it_;
float angle=0.0;
RGB_GRAY()
:it_(nh_)
{
}
~RGB_GRAY()
{
}
};
void rgb_callback(const sensor_msgs::ImageConstPtr& msg)
{
//ROS_INFO("rgb callback");
cv::Mat image;
try
{
image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8) -> image;
char key;
key=cvWaitKey(33);
if(key==32) //the Ascii of "Space key" is 32
saveCloud = true;
if(saveCloud)
{
stringstream stream;
stringstream stream1;
stream <<"Goal RgbImage" << fileNum<<".jpg";
stream1 <<"/home/leonsun/catkin_navi/imgs/" << fileNum <<".jpg";
string filename = stream.str();
string filename1 = stream1.str();
imwrite(filename1,cv_bridge::toCvShare(msg)->image);
saveCloud = false;
fileNum++;
cout << filename << " had Saved."<< endl;
}
}
catch(cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
int width,height;
width=image.cols/4;
height=image.rows/4;
cv::circle(image, cvPoint(image.cols / 2, image.rows / 2), 2, cv::Scalar(255, 0, 0), -1);
cv::Rect rect(image.cols/2-width/2, image.rows/2-height/2, width, height);//左上坐标(x,y)和矩形的长(x)宽(y)
cv::rectangle(image, rect, cv::Scalar(255, 0, 0),1, cv::LINE_8,0);
float x_col,y_row;
x_col=100*sin(angle_wall)+image.cols / 2;
y_row=-100*cos(angle_wall)+image.rows / 2;
cv::line(image,cvPoint(image.cols / 2, image.rows / 2),cvPoint(x_col,y_row),cv::Scalar(255, 0, 0));//画直线
cv::imshow("image", image);
cv::waitKey(5);
}
void depth_callback(const sensor_msgs::ImageConstPtr& msg)
{
//ROS_INFO("depth callback");
cv::Mat image;
try
{
image = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1) -> image; //image.type==CV32FC1 5
}
catch(cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}
int width,height;
width=image.cols/4;
height=image.rows/4;
float a,b,a_depth,b_depth,delta,distance;
a=image.cols/2-width/2;
b=image.cols/2+width/2;
int u_num=height;
int v_num=width;
pcl::PointCloud<pcl::PointXYZ> cloud;
sensor_msgs::PointCloud2 output;
cloud.width = width;
cloud.height = height;
cloud.points.resize(width*height);
float Z,X,Y;
std::vector<cv::Point3f> points;
//MatrixXd positions;
int num=0;
int points_num=0;
float X_sum=0;
float Y_sum=0;
float Z_sum=0;
for(int u=image.rows/2-height/2;u<image.rows/2+height/2;u++)
{
for(int v=image.cols/2-width/2;v<image.cols/2+width/2;v++)
{
cloud.points[num].z=image.at<float>(u,v)/factor;
Z=image.at<float>(u,v)/factor;
cloud.points[num].x=(u - cx) * Z / fx;
cloud.points[num].y=(v - cy) * Z / fy;
X_sum=X_sum+cloud.points[num].x;
Y_sum=Y_sum+cloud.points[num].y;
Z_sum=Z_sum+cloud.points[num].z;
if (Z!=0)
points_num=points_num+1;
num=num+1;
}
}
if (points_num!=0)
{
MatrixXf positions(points_num,3);
float X_mean=X_sum/points_num;
float Y_mean=Y_sum/points_num;
float Z_mean=Z_sum/points_num;
int b=0;
for (int number=0;number<cloud.size();number++)
{
if (cloud.points[number].z!=0)
{
positions(b,0)=cloud.points[number].x-X_mean;
positions(b,1)=cloud.points[number].y-Y_mean;
positions(b,2)=cloud.points[number].z-Z_mean;
b++;
}
}
MatrixXf Covariance_matrix(3,3);
Covariance_matrix=positions.transpose()*positions;
JacobiSVD<Eigen::MatrixXf> svd(Covariance_matrix, ComputeThinU | ComputeThinV );
Matrix3f V = svd.matrixV(), U = svd.matrixU();
Matrix3f S = U.inverse() * Covariance_matrix * V.transpose().inverse(); // S = U^-1 * A * VT * -1
//std::cout<<"A :\n"<<positions<<std::endl;
//cout<<Covariance_matrix<<endl;
//std::cout<<"U :\n"<<U<<std::endl;
//std::cout<<"S :\n"<<S<<std::endl;
//std::cout<<"V :\n"<<V<<std::endl;
Vector3f Fn;
if (V(2,2)<0)
{
Fn(2)=-V(2,2);
Fn(0)=-V(2,0);
Fn(1)=-V(2,1);
}
else
{
Fn(2)=V(2,2);
Fn(0)=V(2,0);
Fn(1)=V(2,1);
}
angle_wall=atan(Fn(0)/Fn(2));
ROS_INFO("Angle: %f", angle_wall*180/PI);
}
else
{
ROS_INFO("Please Dont stand infront of camera.");
}
cv::circle(image, cvPoint(image.cols / 2, image.rows / 2), 2, cv::Scalar(0, 0, 0), -1);
cv::Rect rect(image.cols/2-width/2, image.rows/2-height/2, width, height);//左上坐标(x,y)和矩形的长(x)宽(y)
cv::rectangle(image, rect, cv::Scalar(0, 0, 0),1, cv::LINE_8,0);
cv::imshow("depth", image);
cv::waitKey(5);
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "RGB");
ros::NodeHandle nh_;
//image_transport::ImageTransport it_(nh_);
ros::Subscriber image_sub_,depth_sub_;
cv::namedWindow("image");
cv::namedWindow("depth");
image_sub_ = nh_.subscribe("/camera/rgb/image_raw", 1, rgb_callback);
depth_sub_ = nh_.subscribe("/camera/depth_registered/image_raw", 1, depth_callback);
int err;
pthread_t id;
ros::spin();
}