Overview
This article will illustrate how to feed the raw data from RealSense driver (in Linux) to the Point Cloud Library.Background
One can apply the RealSense SDK to perform some fascinating features in their application with ease. Unfortunately, if you develop your APP in Linux, there's no such good things (at least when I wrote this article). One way to go further is to transport the raw data from RealSense driver to OpenCV or Point Cloud Library (PCL). There're some hints for the former such as here (see Lesly Z. Thu, 08/11/2016 - 03:29), while the tutorial of the latter can't be found easily and is what this article aimed for.Solution
There's an example shipped with the RealSense driver that teach you how to build PointCloud from libRealSense. However, if you want to apply the PointCloudLibrary utilities to it, you should further convert it to the PCL format.In this article, I wrap such conversion and the device data extraction in a class called RealSenseController, which can be used as follows:
RealSenseController realSenseDevice;
while(true)
{
realSenseDevice.Update();
realSenseDevice.CalculatePointCloud();
pcl::PointCloud<pointxyzrgb>::Ptr currentPointCloud = realSenseDevice.GetPointCloud();
DoSomeThing(currentPointCloud);
}
in which the RealSenseController::Update() will update the current depth and color image from the device, and RealSenseControllers::CalculatePointCloud() will compute the PointCloud base on such depth and color image.
This class is defined as below:
class RealSenseController
{
public:
void Update();
const cv::Mat& GetColorImage() const;
const cv::Mat& GetDepthImage() const;
const pcl::PointCloud<pointxyzrgb>::Ptr GetPointCloud() const;
void CalculatePointCloud();
RealSenseController();
private:
rs::context deviceList;
rs::device* cameraDevice;
rs::intrinsics depthIntrinsicTransform;
rs::extrinsics depthToColorTransform;
rs::intrinsics colorIntrinsicTransform;
cv::Mat currentColorImage;
cv::Mat currentDepthImage;
pcl::PointCloud<pointxyzrgb>::Ptr pointCloud;
void initializeDevicesController();
void initializeCameraDevice();
void enableALlVedioStreams();
void initializeDeviceTransforms();
void updateColorImage();
void updateDepthImage();
rs::float3 getDepthPosition3D(size_t rowIndex_, size_t columnIndex_);
cv::Vec3b getColorRelateToDepthPosition(rs::float3 depthPosition3D_) const;
bool isColorPositionValid(rs::int2 colorPosition2D_) const;
rs::int2 convertToNormalPixelFormat(const rs::float2& colorPosition2Dfloat) const;
PointXYZRGB createPointXYZRGB(rs::float3 position3D_, cv::Vec3b pointColorBGR_) const;
float getDepthValueInMeters(size_t rowIndex_, size_t columnIndex_) const;
};
Following is the initialization of the RealSenseController:
RealSenseController::RealSenseController()
: deviceList(),
cameraDevice(),
depthIntrinsicTransform(),
depthToColorTransform(),
colorIntrinsicTransform(),
currentColorImage(),
currentDepthImage(),
pointCloud(new pcl::PointCloud<pointxyzrgb>() )
{
this->initializeDevicesControlCenter();
this->initializeCameraDevice();
this->enableALlVedioStreams();
this->cameraDevice->start();
this->initializeDeviceTransforms();
}
void RealSenseController::initializeDevicesController()
{
int numberOfRealSenseDevicesConnected = this->deviceList.get_device_count();
if (numberOfRealSenseDevicesConnected == 0)
throw RealSenseNotConnectedException();
}
void RealSenseController::initializeCameraDevice()
{
this->cameraDevice = this->deviceList.get_device(0);
}
void RealSenseController::enableALlVedioStreams()
{
this->cameraDevice->enable_stream(rs::stream::color, rs::preset::best_quality);
this->cameraDevice->enable_stream(rs::stream::depth, rs::preset::best_quality);
}
void RealSenseController::initializeDeviceTransforms()
{
this->depthIntrinsicTransform = this->cameraDevice->get_stream_intrinsics(rs::stream::depth);
this->depthToColorTransform = this->cameraDevice->get_extrinsics(rs::stream::depth, rs::stream::color);
this->colorIntrinsicTransform = this->cameraDevice->get_stream_intrinsics(rs::stream::color);
}
The following function will update the color and depth image from the device and store it to the cv::Mat in OpenCV, since the format of the device raw data and cv::Mat are similar and can be expected to convert efficiently.
void RealSenseController::Update()
{
try
{
this->cameraDevice->wait_for_frames();
}
catch(...)
{
std::cerr<<"Device->wait_for_frames() times out\n";
}
this->updateColorImage();
this->updateDepthImage();
}
void RealSenseController::updateColorImage()
{
const void* deviceColorData = this->cameraDevice->get_frame_data(rs::stream::color);
cv::Mat colorTemp = cv::Mat( this->colorIntrinsicTransform.height, this->colorIntrinsicTransform.width, CV_8UC3, const_cast<void*>(deviceColorData) );
cv::cvtColor(colorTemp, this->currentColorImage, CV_RGB2BGR);
}
void RealSenseController::updateDepthImage()
{
const void* deviceDepthData = this->cameraDevice->get_frame_data(rs::stream::depth);
this->currentDepthImage = cv::Mat(this->depthIntrinsicTransform.height, this->depthIntrinsicTransform.width, CV_16UC1, const_cast<void*>(deviceDepthData) );
}
Note: 1. One should call rs::device::wait_for_frames() before getting any data from device.
Moreover, that function might throw exception if device times out (no data received from
device over some period). In this case, I just ignore it and keep trying to get data. Most of
the time, the device will recover and continue to send data.
2. The argument order of the cv::Mat constructor is column number first then comes the row
number.
3. The type of device raw data is "const void*", one should do const_cast<>() before feed it
into cv::Mat.
4. The format of the color data is RGB, and should be changed to BGR which is widely
adopted by OpenCV.
Following code block perform the transformation from 2D depth and color image to form a PointCloud:
void RealSenseController::CalculatePointCloud()
{
if ( (not this->currentColorImage.empty())and(not this->currentDepthImage.empty() ) )
{
this->pointCloud->clear();
for (size_t i=0; i < (size_t)this->currentColorImage.cols; ++i)
{
for (size_t j=0; j < (size_t)this->currentColorImage.rows; ++j)
{
rs::float3 depthPosition3D = getDepthPosition3D(i, j);
try
{
cv::Vec3b colorRelativeToDepth = getColorRelateToDepthPosition(depthPosition3D);
PointXYZRGB currentPoint = createPointXYZRGB(depthPosition3D, colorRelativeToDepth);
this->pointCloud->push_back( currentPoint );
}
catch(const InvalidPointException& error)
{
// Neglect the NAN point
continue;
}
}
}
}
else
{
std::cerr<<"Color or Depth Image is empty, ignore the calculation this frame.\n";
}
}
rs::float3 RealSenseController::getDepthPosition3D(size_t rowIndex_, size_t columnIndex_)
{
rs::float2 depthPosition2D = { (float)columnIndex_, (float)rowIndex_ };
float depthValue = getDepthValueInMeters(rowIndex_, columnIndex_);
return this->depthIntrinsicTransform.deproject(depthPosition2D, depthValue);
}
///
/// This function will throw InvalidPointException, if the Point is invalid (such as there's no match from depth to color pixel.
///
cv::Vec3b RealSenseController::getColorRelateToDepthPosition(rs::float3 depthPosition3D_) const
{
rs::float3 colorPosition3D_RelativeTo_depthPosition = this->depthToColorTransform.transform(depthPosition3D_);
rs::float2 colorPosition2Dfloat = this->colorIntrinsicTransform.project(colorPosition3D_RelativeTo_depthPosition);
rs::int2 colorPosition2Dint = this->convertToNormalPixelFormat(colorPosition2Dfloat);
if ( this->isColorPositionValid(colorPosition2Dint) )
{
return this->currentColorImage.at<cv::vec3b>( colorPosition2Dint.y, colorPosition2Dint.x );
}
else
{
/*
The current Point do not have meaning, and should be ignored.
*/
throw InvalidPointException();
}
}
bool RealSenseController::isColorPositionValid(rs::int2 colorPosition2D_) const
{
return ( (colorPosition2D_.x >= 0)and(colorPosition2D_.y >= 0)
and(colorPosition2D_.x < this->colorIntrinsicTransform.width)
and(colorPosition2D_.y < this->colorIntrinsicTransform.height) );
}
rs::int2 RealSenseController::convertToNormalPixelFormat(const rs::float2& colorPosition2Dfloat) const
{
rs::int2 colorPosition2Dint;
colorPosition2Dint.x = (int)std::round(colorPosition2Dfloat.x);
colorPosition2Dint.y = (int)std::round(colorPosition2Dfloat.y);
return colorPosition2Dint;
}
PointXYZRGB RealSenseController::createPointXYZRGB(rs::float3 position3D_, cv::Vec3b pointColorBGR_) const
{
PointXYZRGB tempPoint;
tempPoint.x = position3D_.x; tempPoint.y = position3D_.y; tempPoint.z = position3D_.z;
tempPoint.r = pointColorBGR_[2]; tempPoint.g = pointColorBGR_[1]; tempPoint.b = pointColorBGR_[0];
return tempPoint;
}
float RealSenseController::getDepthValueInMeters(size_t rowIndex_, size_t columnIndex_) const
{
float depthScale = this->cameraDevice->get_depth_scale();
size_t MAX_ROW_INDEX = (size_t)this->currentDepthImage.rows;
size_t MAX_COLUMN_INDEX = (size_t)this->currentDepthImage.cols;
if ( ( rowIndex_ < MAX_ROW_INDEX )and(columnIndex_ < MAX_COLUMN_INDEX) )
{
return (float)this->currentDepthImage.at(rowIndex_, columnIndex_) * depthScale;
}
else
{
return 0.0f;
}
}
The procedure of building PointCloud can be briefly summarized as follows: 1. Convert the 2D position in depth image to 3D depth position. 2. Transform such 3D depth position to the 3D position in the color coordinate. 3. Transform that position into 2D position in the color image, and get the color relative to the position. 4. Now, you have the 3D position and its color. The PointXYZRGB can be built from that.
Note: 1. For convenience, I define a simple structure that mimics the rs::float2 to store the 2D
position of the color image:
namespace rs
{
struct int2 { int x, y; };
};
2. x is column, y is row.
3. There might be some depth points that do not have their counterparts in the color image. At
this circumstance, the example of libRealSense seems to artificially assign it as white.
However, this will result in too many invalid points near the origin (in my case, the final
point cloud has 307K points while there're 279K points near the origin and has its color
been artificially assigned as white). It will be a disaster if you want to calculate the normals
of that cloud (the calculation time will be at least 10 minutes). The similar issues have been
discussed here. Therefore, I neglect such invalid points by throwing an
InvalidPointException which will be caught and ignored by the
RealSenseController::CalculatePointCloud() and continue the loop.
3. Finally, you might want to remove the outliers.
(a) The calculated point cloud (with points 38.2K):
(b) The above point cloud with its outliers removed (with points: 37.9K):
沒有留言:
張貼留言