i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-01 17:10:26 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added bilateral filtering option. CloudViewer: lighting and edge visibility options.

Browse Source
This commit is contained in:
matlabbe
2016-11-03 17:04:31 -04:00
parent 70dbac10c7
commit a7971373a6
14 changed files with 593 additions and 28 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
corelib/src/CameraThread.cpp
View File

@@ -58,7 +58,10 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_stereoDense(new StereoBM(parameters)),
_distortionModel(0)
_distortionModel(0),
_bilateralFiltering(false),
_bilateralSigmaS(10),
_bilateralSigmaR(0.1)
{
UASSERT(_camera != 0);
}
@@ -106,6 +109,14 @@ void CameraThread::setDistortionModel(const std::string & path)
}
}
void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
{
UASSERT(sigmaS > 0.0f && sigmaR > 0.0f);
_bilateralFiltering = true;
_bilateralSigmaS = sigmaS;
_bilateralSigmaR = sigmaR;
}
void CameraThread::mainLoop()
{
UTimer totalTime;
@@ -139,6 +150,13 @@ void CameraThread::mainLoop()
info.timeUndistortDepth = timer.ticks();
}
if(_bilateralFiltering && !data.depthRaw().empty())
{
UTimer timer;
data.setDepthOrRightRaw(util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR));
info.timeBilateralFiltering = timer.ticks();
}
if(_imageDecimation>1 && !data.imageRaw().empty())
{
UDEBUG("");

240
corelib/src/util2d.cpp
View File

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/video/tracking.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <map>
#include <Eigen/Core>
namespace rtabmap
{
@@ -1622,6 +1623,245 @@ void fillRegisteredDepthHoles(cv::Mat & registeredDepth, bool vertical, bool hor
}
}
// used only for fastBilateralFiltering() below
class Array3D
{
public:
Array3D (const size_t width, const size_t height, const size_t depth)
{
x_dim_ = width;
y_dim_ = height;
z_dim_ = depth;
v_ = std::vector<Eigen::Vector2f> (width*height*depth, Eigen::Vector2f (0.0f, 0.0f));
}
inline Eigen::Vector2f&
operator () (const size_t x, const size_t y, const size_t z)
{ return v_[(x * y_dim_ + y) * z_dim_ + z]; }
inline const Eigen::Vector2f&
operator () (const size_t x, const size_t y, const size_t z) const
{ return v_[(x * y_dim_ + y) * z_dim_ + z]; }
inline void
resize (const size_t width, const size_t height, const size_t depth)
{
x_dim_ = width;
y_dim_ = height;
z_dim_ = depth;
v_.resize (x_dim_ * y_dim_ * z_dim_);
}
Eigen::Vector2f
trilinear_interpolation (const float x,
const float y,
const float z)
{
const size_t x_index = clamp (0, x_dim_ - 1, static_cast<size_t> (x));
const size_t xx_index = clamp (0, x_dim_ - 1, x_index + 1);
const size_t y_index = clamp (0, y_dim_ - 1, static_cast<size_t> (y));
const size_t yy_index = clamp (0, y_dim_ - 1, y_index + 1);
const size_t z_index = clamp (0, z_dim_ - 1, static_cast<size_t> (z));
const size_t zz_index = clamp (0, z_dim_ - 1, z_index + 1);
const float x_alpha = x - static_cast<float> (x_index);
const float y_alpha = y - static_cast<float> (y_index);
const float z_alpha = z - static_cast<float> (z_index);
return
(1.0f-x_alpha) * (1.0f-y_alpha) * (1.0f-z_alpha) * (*this)(x_index, y_index, z_index) +
x_alpha * (1.0f-y_alpha) * (1.0f-z_alpha) * (*this)(xx_index, y_index, z_index) +
(1.0f-x_alpha) * y_alpha * (1.0f-z_alpha) * (*this)(x_index, yy_index, z_index) +
x_alpha * y_alpha * (1.0f-z_alpha) * (*this)(xx_index, yy_index, z_index) +
(1.0f-x_alpha) * (1.0f-y_alpha) * z_alpha * (*this)(x_index, y_index, zz_index) +
x_alpha * (1.0f-y_alpha) * z_alpha * (*this)(xx_index, y_index, zz_index) +
(1.0f-x_alpha) * y_alpha * z_alpha * (*this)(x_index, yy_index, zz_index) +
x_alpha * y_alpha * z_alpha * (*this)(xx_index, yy_index, zz_index);
}
static inline size_t
clamp (const size_t min_value,
const size_t max_value,
const size_t x)
{
if (x >= min_value && x <= max_value)
{
return x;
}
else if (x < min_value)
{
return (min_value);
}
else
{
return (max_value);
}
}
inline size_t
x_size () const
{ return x_dim_; }
inline size_t
y_size () const
{ return y_dim_; }
inline size_t
z_size () const
{ return z_dim_; }
inline std::vector<Eigen::Vector2f >::iterator
begin ()
{ return v_.begin (); }
inline std::vector<Eigen::Vector2f >::iterator
end ()
{ return v_.end (); }
inline std::vector<Eigen::Vector2f >::const_iterator
begin () const
{ return v_.begin (); }
inline std::vector<Eigen::Vector2f >::const_iterator
end () const
{ return v_.end (); }
private:
std::vector<Eigen::Vector2f > v_;
size_t x_dim_, y_dim_, z_dim_;
};
/**
* Converted pcl::FastBilateralFiltering class to 2d depth image
*/
cv::Mat fastBilateralFiltering(const cv::Mat & depth, float sigmaS, float sigmaR, bool earlyDivision)
{
UASSERT(!depth.empty() && (depth.type() == CV_32FC1 || depth.type() == CV_16UC1));
UDEBUG("Begin: depth float=%d %dx%d sigmaS=%f sigmaR=%f earlDivision=%d",
depth.type()==CV_32FC1?1:0, depth.cols, depth.rows, sigmaS, sigmaR, earlyDivision?1:0);
cv::Mat output = depth.clone();
float base_max = -std::numeric_limits<float>::max ();
float base_min = std::numeric_limits<float>::max ();
bool found_finite = false;
for (size_t x = 0; x < output.cols; ++x)
for (size_t y = 0; y < output.rows; ++y)
{
float z = depth.type()==CV_32FC1?output.at<float>(y, x):float(output.at<unsigned short>(y, x))/1000.0f;
if (z > 0.0f && uIsFinite(z))
{
if (base_max < z)
base_max = z;
if (base_min > z)
base_min = z;
found_finite = true;
}
}
if (!found_finite)
{
UWARN("Given an empty depth image. Doing nothing.");
return cv::Mat();
}
UDEBUG("base_min=%f base_max=%f", base_min, base_max);
const float base_delta = base_max - base_min;
const size_t padding_xy = 2;
const size_t padding_z = 2;
const size_t small_width = static_cast<size_t> (static_cast<float> (depth.cols - 1) / sigmaS) + 1 + 2 * padding_xy;
const size_t small_height = static_cast<size_t> (static_cast<float> (depth.rows - 1) / sigmaS) + 1 + 2 * padding_xy;
const size_t small_depth = static_cast<size_t> (base_delta / sigmaR) + 1 + 2 * padding_z;
UDEBUG("small_width=%d small_height=%d small_depth=%d", (int)small_width, (int)small_height, (int)small_depth);
Array3D data (small_width, small_height, small_depth);
for (size_t x = 0; x < depth.cols; ++x)
{
const size_t small_x = static_cast<size_t> (static_cast<float> (x) / sigmaS + 0.5f) + padding_xy;
for (size_t y = 0; y < depth.rows; ++y)
{
float v = depth.type()==CV_32FC1?output.at<float>(y,x):float(output.at<unsigned short>(y,x))/1000.0f;
if((v > 0 && uIsFinite(v)))
{
float z = v - base_min;
const size_t small_y = static_cast<size_t> (static_cast<float> (y) / sigmaS + 0.5f) + padding_xy;
const size_t small_z = static_cast<size_t> (static_cast<float> (z) / sigmaR + 0.5f) + padding_z;
Eigen::Vector2f& d = data (small_x, small_y, small_z);
d[0] += v;
d[1] += 1.0f;
}
}
}
std::vector<long int> offset (3);
offset[0] = &(data (1,0,0)) - &(data (0,0,0));
offset[1] = &(data (0,1,0)) - &(data (0,0,0));
offset[2] = &(data (0,0,1)) - &(data (0,0,0));
Array3D buffer (small_width, small_height, small_depth);
for (size_t dim = 0; dim < 3; ++dim)
{
const long int off = offset[dim];
for (size_t n_iter = 0; n_iter < 2; ++n_iter)
{
std::swap (buffer, data);
for(size_t x = 1; x < small_width - 1; ++x)
for(size_t y = 1; y < small_height - 1; ++y)
{
Eigen::Vector2f* d_ptr = &(data (x,y,1));
Eigen::Vector2f* b_ptr = &(buffer (x,y,1));
for(size_t z = 1; z < small_depth - 1; ++z, ++d_ptr, ++b_ptr)
*d_ptr = (*(b_ptr - off) + *(b_ptr + off) + 2.0 * (*b_ptr)) / 4.0;
}
}
}
if (earlyDivision)
{
for (std::vector<Eigen::Vector2f, Eigen::aligned_allocator<Eigen::Vector2f> >::iterator d = data.begin (); d != data.end (); ++d)
*d /= ((*d)[0] != 0) ? (*d)[1] : 1;
}
for (size_t x = 0; x < depth.cols; ++x)
for (size_t y = 0; y < depth.rows; ++y)
{
float z = depth.type()==CV_32FC1?output.at<float>(y,x):float(output.at<unsigned short>(y,x))/1000.0f;
if(z > 0 && uIsFinite(z))
{
z -= base_min;
const Eigen::Vector2f D = data.trilinear_interpolation (static_cast<float> (x) / sigmaS + padding_xy,
static_cast<float> (y) / sigmaS + padding_xy,
z / sigmaR + padding_z);
float v = earlyDivision ? D[0] : D[0] / D[1];
if(v < base_min || v >= base_max)
{
v = 0.0f;
}
if(depth.type()==CV_32FC1)
output.at<float>(y,x) = v;
else
{
v*=1000.0f;
if(v>65535.0f)
{
v = 65535.0f;
}
output.at<unsigned short>(y,x) = v;
}
}
}
UDEBUG("End");
return output;
}
}
}