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

CMake: added WITH_OPENMP option (to be able to disable it). CameraOpenNI2: Added depth decimation parameter. CLAMS: can apply distortion model to smaller images.

Browse Source
This commit is contained in:
matlabbe
2021-09-24 17:31:16 -04:00
parent bccc5b13af
commit 8c56b5b1ce
8 changed files with 176 additions and 121 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/include/rtabmap/core/camera/CameraOpenNI2.h
View File

@@ -68,6 +68,7 @@ public:
bool setMirroring(bool enabled);
void setOpenNI2StampsAndIDsUsed(bool used);
void setIRDepthShift(int horizontal, int vertical);
void setDepthDecimation(int decimation);
protected:
virtual SensorData captureImage(CameraInfo * info = 0);
@@ -85,6 +86,7 @@ private:
StereoCameraModel _stereoModel;
int _depthHShift;
int _depthVShift;
int _depthDecimation;
#endif
};

2
corelib/src/Camera.cpp
View File

@@ -99,7 +99,7 @@ SensorData Camera::takeImage(CameraInfo * info)
}
UTimer timer;
SensorData data = this->captureImage(info);
SensorData data = this->captureImage(info);
double captureTime = timer.ticks();
if(warnFrameRateTooHigh)
{

8
corelib/src/CameraThread.cpp
View File

@@ -365,8 +365,10 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
if(_distortionModel && !data.depthRaw().empty())
{
UTimer timer;
if(_distortionModel->getWidth() == data.depthRaw().cols &&
_distortionModel->getHeight() == data.depthRaw().rows )
if(_distortionModel->getWidth() >= data.depthRaw().cols &&
_distortionModel->getHeight() >= data.depthRaw().rows &&
_distortionModel->getWidth() % data.depthRaw().cols == 0 &&
_distortionModel->getHeight() % data.depthRaw().rows == 0)
{
cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures.
_distortionModel->undistort(depth);
@@ -374,7 +376,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
}
else
{
UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!",
UERROR("Distortion model size is %dx%d but depth image is %dx%d!",
_distortionModel->getWidth(), _distortionModel->getHeight(),
data.depthRaw().cols, data.depthRaw().rows);
}

23
corelib/src/camera/CameraOpenNI2.cpp
View File

@@ -74,7 +74,8 @@ CameraOpenNI2::CameraOpenNI2(
_deviceId(deviceId),
_openNI2StampsAndIDsUsed(false),
_depthHShift(0),
_depthVShift(0)
_depthVShift(0),
_depthDecimation(1)
#endif
{
}
@@ -184,6 +185,14 @@ void CameraOpenNI2::setIRDepthShift(int horizontal, int vertical)
#endif
}
void CameraOpenNI2::setDepthDecimation(int decimation)
{
#ifdef RTABMAP_OPENNI2
UASSERT(decimation >= 1);
_depthDecimation = decimation;
#endif
}
bool CameraOpenNI2::init(const std::string & calibrationFolder, const std::string & cameraName)
{
#ifdef RTABMAP_OPENNI2
@@ -544,8 +553,18 @@ SensorData CameraOpenNI2::captureImage(CameraInfo * info)
if(_stereoModel.left().isValidForRectification() && !_stereoModel.stereoTransform().isNull())
{
depth = _stereoModel.left().rectifyImage(depth, 0);
depth = util2d::registerDepth(depth, _stereoModel.left().K(), rgb.size(), _stereoModel.right().K(), _stereoModel.stereoTransform());
CameraModel depthModel = _stereoModel.left().scaled(1.0 / double(_depthDecimation));
depth = util2d::decimate(depth, _depthDecimation);
depth = util2d::registerDepth(depth, depthModel.K(), rgb.size()/_depthDecimation, _stereoModel.right().scaled(1.0/double(_depthDecimation)).K(), _stereoModel.stereoTransform());
}
else if (_depthDecimation > 1)
{
depth = util2d::decimate(depth, _depthDecimation);
}
}
else if (_depthDecimation > 1)
{
depth = util2d::decimate(depth, _depthDecimation);
}
}
else // IR

18
corelib/src/clams/discrete_depth_distortion_model.cpp
View File

@@ -275,19 +275,21 @@ namespace clams
void DiscreteDepthDistortionModel::undistort(cv::Mat & depth) const
{
UASSERT(width_ == depth.cols);
UASSERT(height_ ==depth.rows);
UASSERT(width_ >= depth.cols && width_ % depth.cols == 0);
UASSERT(height_ >=depth.rows && height_ % depth.rows == 0);
UASSERT(height_ >= depth.rows && height_ % depth.rows == 0);
UASSERT(depth.type() == CV_16UC1 || depth.type() == CV_32FC1);
int factor = width_ / depth.cols;
if(depth.type() == CV_32FC1)
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
for(int v = 0; v < depth.rows; ++v) {
for(int u = 0; u < depth.cols; ++u) {
float & z = depth.at<float>(v, u);
if(uIsNan(z) || z == 0.0f)
continue;
double zf = z;
frustum(v, u).interpolatedUndistort(&zf);
frustum(v * factor, u * factor).interpolatedUndistort(&zf);
z = zf;
}
}
@@ -295,13 +297,13 @@ namespace clams
else
{
#pragma omp parallel for
for(int v = 0; v < height_; ++v) {
for(int u = 0; u < width_; ++u) {
for(int v = 0; v < depth.rows; ++v) {
for(int u = 0; u < depth.cols; ++u) {
unsigned short & z = depth.at<unsigned short>(v, u);
if(uIsNan(z) || z == 0)
continue;
double zf = z * 0.001;
frustum(v, u).interpolatedUndistort(&zf);
frustum(v * factor, u * factor).interpolatedUndistort(&zf);
z = zf*1000;
}
}