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

Fixed PnP camera matrix empty when computing loop closure. Updated odometry nonholomic motion estimation (using arc around ICR).

Browse Source
This commit is contained in:
matlabbe
2015-06-18 01:24:19 -04:00
parent e9bb80abcc
commit 82943e85e8
3 changed files with 43 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
corelib/src/CameraThread.cpp
View File

@@ -127,7 +127,9 @@ void CameraThread::mainLoop()
if(_cameraRGBD) if(_cameraRGBD)
{ {
SensorData data; SensorData data;
if(dynamic_cast<CameraStereoDC1394*>(_cameraRGBD) || dynamic_cast<CameraStereoDC1394*>(_cameraRGBD)) if(dynamic_cast<CameraStereoFlyCapture2*>(_cameraRGBD) ||
dynamic_cast<CameraStereoDC1394*>(_cameraRGBD) ||
dynamic_cast<CameraStereoImages*>(_cameraRGBD))
{ {
//stereo //stereo
data = SensorData(rgb, depth, StereoCameraModel(fx, fx, cx, cy, fyOrBaseline, _cameraRGBD->getLocalTransform()), ++_seq, stamp); data = SensorData(rgb, depth, StereoCameraModel(fx, fx, cx, cy, fyOrBaseline, _cameraRGBD->getLocalTransform()), ++_seq, stamp);

48
corelib/src/Memory.cpp
View File

@@ -1975,20 +1975,29 @@ Transform Memory::computeVisualTransform(
UWARN("PnP estimation and Epipolar geometry estimation are set, only PnP is used."); UWARN("PnP estimation and Epipolar geometry estimation are set, only PnP is used.");
} }
if(!newS.sensorData().rightRaw().empty() && !newS.sensorData().stereoCameraModel().isValid()) if((!newS.sensorData().rightRaw().empty() ||
{ !newS.sensorData().stereoCameraModel().isValid()) &&
UERROR("Calibrated stereo camera required"); (!newS.sensorData().depthRaw().empty() ||
} newS.sensorData().cameraModels().size() != 1 ||
else if(!newS.sensorData().depthRaw().empty() && !newS.sensorData().cameraModels()[0].isValid()))
(newS.sensorData().cameraModels().size() != 1 || !newS.sensorData().cameraModels()[0].isValid()))
{ {
UERROR("Calibrated camera required (multi-cameras not supported)."); UERROR("Calibrated camera required (multi-cameras not supported).");
} }
else else
{ {
cv::Mat K = newS.sensorData().cameraModels().size()?newS.sensorData().cameraModels()[0].K():newS.sensorData().stereoCameraModel().left().K(); cv::Mat K;
Transform localTransform = newS.sensorData().cameraModels().size()?newS.sensorData().cameraModels()[0].localTransform():newS.sensorData().stereoCameraModel().left().localTransform(); Transform localTransform;
if(newS.sensorData().cameraModels().size())
{
K = newS.sensorData().cameraModels()[0].K();
localTransform = newS.sensorData().cameraModels()[0].localTransform();
}
else
{
K = newS.sensorData().stereoCameraModel().left().K();
localTransform = newS.sensorData().stereoCameraModel().left().localTransform();
}
UASSERT(!K.empty() && !localTransform.isNull());
// 2D -> 3D // 2D -> 3D
if(!oldS.getWords3().empty() && !newS.getWords().empty()) if(!oldS.getWords3().empty() && !newS.getWords().empty())
{ {
@@ -4268,11 +4277,22 @@ Signature * Memory::createSignature(const SensorData & data, const Transform & p
data.stamp(), data.stamp(),
"", "",
pose, pose,
data.userData(), data.userData(),
SensorData( stereoCameraModel.isValid()?
rtabmap::compressData2(laserScan), SensorData(
data.laserScanMaxPts(), rtabmap::compressData2(laserScan),
cv::Mat(), cv::Mat(), CameraModel(), id)); data.laserScanMaxPts(),
cv::Mat(),
cv::Mat(),
stereoCameraModel,
id):
SensorData(
rtabmap::compressData2(laserScan),
data.laserScanMaxPts(),
cv::Mat(),
cv::Mat(),
cameraModels,
id));
} }
s->setWords(words); s->setWords(words);
s->setWords3(words3D); s->setWords3(words3D);

10
corelib/src/Odometry.cpp
View File

@@ -218,14 +218,15 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
if(!_holonomic) if(!_holonomic)
{ {
float tmpY = x * tan(yaw); // arc trajectory around ICR
float tmpY = yaw!=0.0f ? x / tan((CV_PI-yaw)/2.0f) : 0.0f;
if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0)) if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
{ {
y = tmpY; y = tmpY;
} }
else else
{ {
yaw = atan(y/x); yaw = (atan(x/y)*2.0f-CV_PI)*-1;
} }
} }
@@ -244,14 +245,15 @@ Transform Odometry::process(const SensorData & data, OdometryInfo * info)
} }
else if(!_holonomic) else if(!_holonomic)
{ {
float tmpY = x * tan(yaw); // arc trajectory around ICR
float tmpY = yaw!=0.0f ? x / tan((CV_PI-yaw)/2.0f) : 0.0f;
if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0)) if(fabs(tmpY) < fabs(y) || (tmpY<=0 && y >=0) || (tmpY>=0 && y<=0))
{ {
y = tmpY; y = tmpY;
} }
else else
{ {
yaw = atan(y/x); yaw = (atan(x/y)*2.0f-CV_PI)*-1;
} }
} }
UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) && UASSERT_MSG(uIsFinite(x) && uIsFinite(y) && uIsFinite(z) &&