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

CameraImages: moved scan filtering stuff to CameraThread

Browse Source 
Preferences->Source: refactored scan filtering parameters
OdometryF2M and OdometryF2F: moved odom orientation init from IMU to OdometryROS instead
This commit is contained in:
matlabbe
2019-01-15 15:41:07 -05:00
parent 42c8ed0c0f
commit 9da2c1918f
12 changed files with 210 additions and 269 deletions
Download Patch File Download Diff File Expand all files Collapse all files

27
corelib/include/rtabmap/core/CameraThread.h
View File

@@ -69,20 +69,24 @@ public:
void enableBilateralFiltering(float sigmaS, float sigmaR);
void disableBilateralFiltering() {_bilateralFiltering = false;}
void setScanFromDepth(
bool enabled,
int decimation=4,
float maxDepth=4.0f,
void setScanParameters(
bool fromDepth,
int downsampleStep=1, // decimation of the depth image in case the scan is from depth image
float rangeMin=0.0f,
float rangeMax=0.0f,
float voxelSize = 0.0f,
int normalsK = 0,
int normalsRadius = 0.0f)
int normalsRadius = 0.0f,
bool forceGroundNormalsUp = false)
{
_scanFromDepth = enabled;
_scanDecimation=decimation;
_scanMaxDepth = maxDepth;
_scanFromDepth = fromDepth;
_scanDownsampleStep=downsampleStep;
_scanRangeMin = rangeMin;
_scanRangeMax = rangeMax;
_scanVoxelSize = voxelSize;
_scanNormalsK = normalsK;
_scanNormalsRadius = normalsRadius;
_scanForceGroundNormalsUp = forceGroundNormalsUp;
}
void postUpdate(SensorData * data, CameraInfo * info = 0) const;
@@ -106,12 +110,13 @@ private:
int _imageDecimation;
bool _stereoToDepth;
bool _scanFromDepth;
int _scanDecimation;
float _scanMaxDepth;
float _scanMinDepth;
int _scanDownsampleStep;
float _scanRangeMin;
float _scanRangeMax;
float _scanVoxelSize;
int _scanNormalsK;
float _scanNormalsRadius;
bool _scanForceGroundNormalsUp;
StereoDense * _stereoDense;
clams::DiscreteDepthDistortionModel * _distortionModel;
bool _bilateralFiltering;

17
corelib/include/rtabmap/core/camera/CameraImages.h
View File

@@ -76,21 +76,11 @@ public:
void setScanPath(
const std::string & dir,
int maxScanPts = 0,
int downsampleStep = 1,
float voxelSize = 0.0f,
int normalsK = 0, // compute normals if > 0
float normalsRadius = 0, // compute normals if > 0
const Transform & localTransform=Transform::getIdentity(),
bool forceGroundNormalsUp = false)
const Transform & localTransform=Transform::getIdentity())
{
_scanPath = dir;
_scanLocalTransform = localTransform;
_scanMaxPts = maxScanPts;
_scanDownsampleStep = downsampleStep;
_scanNormalsK = normalsK;
_scanNormalsRadius = normalsRadius;
_scanVoxelSize = voxelSize;
_scanForceGroundNormalsUp = forceGroundNormalsUp;
}
void setDepthFromScan(bool enabled, int fillHoles = 1, bool fillHolesFromBorder = false)
@@ -152,11 +142,6 @@ private:
std::string _scanPath;
Transform _scanLocalTransform;
int _scanMaxPts;
int _scanDownsampleStep;
float _scanVoxelSize;
int _scanNormalsK;
float _scanNormalsRadius;
bool _scanForceGroundNormalsUp;
bool _depthFromScan;
int _depthFromScanFillHoles; // <0:horizontal 0:disabled >0:vertical

25
corelib/src/CameraThread.cpp
View File

@@ -54,12 +54,13 @@ CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
_imageDecimation(1),
_stereoToDepth(false),
_scanFromDepth(false),
_scanDecimation(4),
_scanMaxDepth(4.0f),
_scanMinDepth(0.0f),
_scanDownsampleStep(1),
_scanRangeMin(0.0f),
_scanRangeMax(0.0f),
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_scanNormalsRadius(0.0f),
_scanForceGroundNormalsUp(false),
_stereoDense(StereoDense::create(parameters)),
_distortionModel(0),
_bilateralFiltering(false),
@@ -321,16 +322,16 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
UDEBUG("");
if(data.laserScanRaw().isEmpty())
{
UASSERT(_scanDecimation >= 1);
UASSERT(_scanDownsampleStep >= 1);
UTimer timer;
pcl::IndicesPtr validIndices(new std::vector<int>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
data,
_scanDecimation,
_scanMaxDepth,
_scanMinDepth,
_scanDownsampleStep,
_scanRangeMax,
_scanRangeMin,
validIndices.get());
float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
float maxPoints = (data.depthRaw().rows/_scanDownsampleStep)*(data.depthRaw().cols/_scanDownsampleStep);
cv::Mat scan;
const Transform & baseToScan = data.cameraModels()[0].localTransform();
LaserScan::Format format = LaserScan::kXYZRGB;
@@ -366,7 +367,7 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
}
}
}
data.setLaserScanRaw(LaserScan(scan, (int)maxPoints, _scanMaxDepth, format, baseToScan));
data.setLaserScanRaw(LaserScan(scan, (int)maxPoints, _scanRangeMax, format, baseToScan));
if(info) info->timeScanFromDepth = timer.ticks();
}
else
@@ -376,6 +377,12 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
"depth will not be created.");
}
}
else if(!data.laserScanRaw().isEmpty())
{
UDEBUG("");
// filter the scan after registration
data.setLaserScanRaw(util3d::commonFiltering(data.laserScanRaw(), _scanDownsampleStep, _scanRangeMin, _scanRangeMax, _scanVoxelSize, _scanNormalsK, _scanNormalsRadius, _scanForceGroundNormalsUp));
}
}
} // namespace rtabmap

12
corelib/src/camera/CameraImages.cpp
View File

@@ -52,11 +52,6 @@ CameraImages::CameraImages() :
_scanDir(0),
_scanLocalTransform(Transform::getIdentity()),
_scanMaxPts(0),
_scanDownsampleStep(1),
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_scanNormalsRadius(0),
_scanForceGroundNormalsUp(false),
_depthFromScan(false),
_depthFromScanFillHoles(1),
_depthFromScanFillHolesFromBorder(false),
@@ -84,11 +79,6 @@ CameraImages::CameraImages(const std::string & path,
_scanDir(0),
_scanLocalTransform(Transform::getIdentity()),
_scanMaxPts(0),
_scanDownsampleStep(1),
_scanVoxelSize(0.0f),
_scanNormalsK(0),
_scanNormalsRadius(0),
_scanForceGroundNormalsUp(false),
_depthFromScan(false),
_depthFromScanFillHoles(1),
_depthFromScanFillHolesFromBorder(false),
@@ -731,8 +721,6 @@ SensorData CameraImages::captureImage(CameraInfo * info)
}
}
}
// filter the scan after registration
scan = util3d::commonFiltering(scan, _scanDownsampleStep, 0, 0, _scanVoxelSize, _scanNormalsK, _scanNormalsRadius, _scanForceGroundNormalsUp);
}
}
else

24
corelib/src/odometry/OdometryF2F.cpp
View File

@@ -91,30 +91,6 @@ Transform OdometryF2F::computeTransform(
UASSERT(!this->getPose().isNull());
if(lastKeyFramePose_.isNull())
{
if(this->getPose().rotation().isIdentity() &&
data.imu().linearAcceleration()[0]!=0.0 &&
data.imu().linearAcceleration()[1]!=0.0 &&
data.imu().linearAcceleration()[2]!=0.0 &&
!data.imu().localTransform().isNull())
{
// align with gravity
Eigen::Vector3f n(data.imu().linearAcceleration()[0], data.imu().linearAcceleration()[1], data.imu().linearAcceleration()[2]);
n = data.imu().localTransform().toEigen3f() * n;
n.normalize();
n[0]*=-1;
n[1]*=-1;
n[2]*=-1;
Eigen::Vector3f z(0,0,1);
//get rotation from z to n;
Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation(
R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), 0);
this->reset(rotation);
}
lastKeyFramePose_ = this->getPose(); // reset to current pose
}
Transform motionSinceLastKeyFrame = lastKeyFramePose_.inverse()*this->getPose();

25
corelib/src/odometry/OdometryF2M.cpp
View File

@@ -938,31 +938,6 @@ Transform OdometryF2M::computeTransform(
regInfo.covariance = cv::Mat::eye(6,6,CV_64FC1)*9999.0;
bool frameValid = false;
if(this->getPose().rotation().isIdentity() &&
data.imu().linearAcceleration()[0]!=0.0 &&
data.imu().linearAcceleration()[1]!=0.0 &&
data.imu().linearAcceleration()[2]!=0.0 &&
!data.imu().localTransform().isNull())
{
// align with gravity
Eigen::Vector3f n(data.imu().linearAcceleration()[0], data.imu().linearAcceleration()[1], data.imu().linearAcceleration()[2]);
n = data.imu().localTransform().toEigen3f() * n;
n.normalize();
n[0]*=-1;
n[1]*=-1;
n[2]*=-1;
Eigen::Vector3f z(0,0,1);
//get rotation from z to n;
Eigen::Matrix3f R;
R = Eigen::Quaternionf().setFromTwoVectors(n,z);
Transform rotation(
R(0,0), R(0,1), R(0,2), 0,
R(1,0), R(1,1), R(1,2), 0,
R(2,0), R(2,1), R(2,2), 0);
this->reset(rotation);
}
Transform newFramePose = this->getPose(); // initial pose may be not identity...
if(regPipeline_->isImageRequired())
{