i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 17:40:23 +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

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())
{