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

Added stereo multi-camera support (#884)

Browse Source 
* Integrated OpenGV

* Fixed build without opengv

* Cmake: moved OpenGV dependency status under solvers group

* Added multi-stereocamera models support

* Fixed OpenGV 0 sample error when one of the camera doesn't have features. Fixed g2o BA id offset with multi-camera.

* Fixed multicam 3d points generated from stereo correspondences

* db: Fixed multi stereo models not loaded correctly

* gui: fixed stereo rectification option, RegVis: fixed projection error with old databases (image size not set in calibration)

* OdomF2M: Fixed map.at error when bundle adjustment is not used

* depthai: added imu firmware update option for convenience

* Fixed various refactor errors

* Moved "large number stereo correspondences rejected" warning outside computeCorrespondences function for multicam

* Added error log if ba correspondences are computed with empty signatures

* fixed compilation errors with latest opencv

Co-authored-by: mathieu86 <mathieu@robust.ai>
This commit is contained in:
matlabbe
2022-07-20 15:20:14 -04:00
committed by GitHub
parent 71a28bb570
commit 5943a8b065
64 changed files with 2205 additions and 1010 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
corelib/src/CMakeLists.txt
View File

@@ -469,6 +469,13 @@ IF(FastCV_FOUND)
)
ENDIF(FastCV_FOUND)
IF(opengv_FOUND)
SET(LIBRARIES
${LIBRARIES}
opengv
)
ENDIF(opengv_FOUND)
IF(PDAL_FOUND)
SET(INCLUDE_DIRS
${INCLUDE_DIRS}

163
corelib/src/CameraThread.cpp
View File

@@ -287,9 +287,10 @@ void CameraThread::mainLoop()
model.setLocalTransform(_extrinsicsOdomToCamera);
data.setCameraModel(model);
}
else
else if(!data.stereoCameraModels().empty())
{
StereoCameraModel model = data.stereoCameraModel();
UASSERT(data.stereoCameraModels().size()==1);
StereoCameraModel model = data.stereoCameraModels()[0];
model.setLocalTransform(_extrinsicsOdomToCamera);
data.setStereoCameraModel(model);
}
@@ -358,7 +359,12 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
}
else if(!data.rightRaw().empty())
{
data.setRGBDImage(data.imageRaw(), cv::Mat(), data.stereoCameraModel().left());
std::vector<CameraModel> models;
for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
{
models.push_back(data.stereoCameraModels()[i].left());
}
data.setRGBDImage(data.imageRaw(), cv::Mat(), models);
}
}
@@ -435,91 +441,116 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
{
data.setRGBDImage(image, depthOrRight, models);
}
else
std::vector<StereoCameraModel> stereoModels = data.stereoCameraModels();
for(unsigned int i=0; i<stereoModels.size(); ++i)
{
StereoCameraModel stereoModel = data.stereoCameraModel();
if(stereoModel.isValidForProjection())
if(stereoModels[i].isValidForProjection())
{
stereoModel.scale(1.0/double(_imageDecimation));
stereoModels[i].scale(1.0/double(_imageDecimation));
}
data.setStereoImage(image, depthOrRight, stereoModel);
}
if(!stereoModels.empty())
{
data.setStereoImage(image, depthOrRight, stereoModels);
}
}
if(info) info->timeImageDecimation = timer.ticks();
}
if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size() == 1)
if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size()>=1)
{
UDEBUG("");
UTimer timer;
cv::Mat tmpRgb;
cv::flip(data.imageRaw(), tmpRgb, 1);
if(data.cameraModels().size() == 1)
{
UDEBUG("");
UTimer timer;
cv::Mat tmpRgb;
cv::flip(data.imageRaw(), tmpRgb, 1);
UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
CameraModel tmpModel = data.cameraModels()[0];
if(data.cameraModels()[0].cx())
{
tmpModel = CameraModel(
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy(),
float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
data.cameraModels()[0].cy(),
data.cameraModels()[0].localTransform(),
data.cameraModels()[0].Tx(),
data.cameraModels()[0].imageSize());
CameraModel tmpModel = data.cameraModels()[0];
if(data.cameraModels()[0].cx())
{
tmpModel = CameraModel(
data.cameraModels()[0].fx(),
data.cameraModels()[0].fy(),
float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
data.cameraModels()[0].cy(),
data.cameraModels()[0].localTransform(),
data.cameraModels()[0].Tx(),
data.cameraModels()[0].imageSize());
}
cv::Mat tmpDepth = data.depthOrRightRaw();
if(!data.depthRaw().empty())
{
cv::flip(data.depthRaw(), tmpDepth, 1);
}
data.setRGBDImage(tmpRgb, tmpDepth, tmpModel);
if(info) info->timeMirroring = timer.ticks();
}
cv::Mat tmpDepth = data.depthOrRightRaw();
if(!data.depthRaw().empty())
else
{
cv::flip(data.depthRaw(), tmpDepth, 1);
UWARN("Mirroring is not implemented for multiple cameras or stereo...");
}
data.setRGBDImage(tmpRgb, tmpDepth, tmpModel);
if(info) info->timeMirroring = timer.ticks();
}
if(_stereoExposureCompensation && !data.imageRaw().empty() && !data.rightRaw().empty())
{
if(data.stereoCameraModels().size()==1)
{
#if CV_MAJOR_VERSION < 3
UWARN("Stereo exposure compensation not implemented for OpenCV version under 3.");
UWARN("Stereo exposure compensation not implemented for OpenCV version under 3.");
#else
UDEBUG("");
UTimer timer;
cv::Ptr<cv::detail::ExposureCompensator> compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN);
std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
std::vector<cv::UMat> images;
std::vector<cv::UMat> masks(2, cv::UMat(data.imageRaw().size(), CV_8UC1, cv::Scalar(255)));
images.push_back(data.imageRaw().getUMat(cv::ACCESS_READ));
images.push_back(data.rightRaw().getUMat(cv::ACCESS_READ));
compensator->feed(topLeftCorners, images, masks);
cv::Mat imgLeft = data.imageRaw().clone();
compensator->apply(0, cv::Point(0,0), imgLeft, masks[0]);
cv::Mat imgRight = data.rightRaw().clone();
compensator->apply(1, cv::Point(0,0), imgRight, masks[1]);
data.setStereoImage(imgLeft, imgRight, data.stereoCameraModel());
cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get();
UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]);
if(info) info->timeStereoExposureCompensation = timer.ticks();
UDEBUG("");
UTimer timer;
cv::Ptr<cv::detail::ExposureCompensator> compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN);
std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
std::vector<cv::UMat> images;
std::vector<cv::UMat> masks(2, cv::UMat(data.imageRaw().size(), CV_8UC1, cv::Scalar(255)));
images.push_back(data.imageRaw().getUMat(cv::ACCESS_READ));
images.push_back(data.rightRaw().getUMat(cv::ACCESS_READ));
compensator->feed(topLeftCorners, images, masks);
cv::Mat imgLeft = data.imageRaw().clone();
compensator->apply(0, cv::Point(0,0), imgLeft, masks[0]);
cv::Mat imgRight = data.rightRaw().clone();
compensator->apply(1, cv::Point(0,0), imgRight, masks[1]);
data.setStereoImage(imgLeft, imgRight, data.stereoCameraModels()[0]);
cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get();
UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]);
if(info) info->timeStereoExposureCompensation = timer.ticks();
#endif
}
else
{
UWARN("Stereo exposure compensation only is not implemented to multiple stereo cameras...");
}
}
if(_stereoToDepth && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
if(_stereoToDepth && !data.imageRaw().empty() && !data.stereoCameraModels().empty() && data.stereoCameraModels()[0].isValidForProjection() && !data.rightRaw().empty())
{
UDEBUG("");
UTimer timer;
cv::Mat depth = util2d::depthFromDisparity(
_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline());
// set Tx for stereo bundle adjustment (when used)
CameraModel model = CameraModel(
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().left().fy(),
data.stereoCameraModel().left().cx(),
data.stereoCameraModel().left().cy(),
data.stereoCameraModel().localTransform(),
-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx(),
data.stereoCameraModel().left().imageSize());
data.setRGBDImage(data.imageRaw(), depth, model);
if(info) info->timeDisparity = timer.ticks();
if(data.stereoCameraModels().size()==1)
{
UDEBUG("");
UTimer timer;
cv::Mat depth = util2d::depthFromDisparity(
_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
data.stereoCameraModels()[0].left().fx(),
data.stereoCameraModels()[0].baseline());
// set Tx for stereo bundle adjustment (when used)
CameraModel model = CameraModel(
data.stereoCameraModels()[0].left().fx(),
data.stereoCameraModels()[0].left().fy(),
data.stereoCameraModels()[0].left().cx(),
data.stereoCameraModels()[0].left().cy(),
data.stereoCameraModels()[0].localTransform(),
-data.stereoCameraModels()[0].baseline()*data.stereoCameraModels()[0].left().fx(),
data.stereoCameraModels()[0].left().imageSize());
data.setRGBDImage(data.imageRaw(), depth, model);
if(info) info->timeDisparity = timer.ticks();
}
else
{
UWARN("Stereo to depth is not implemented for multiple stereo cameras...");
}
}
if(_scanFromDepth &&
data.cameraModels().size() &&

6
corelib/src/DBDriver.cpp
View File

@@ -726,7 +726,7 @@ void DBDriver::getNodeData(
bool DBDriver::getCalibration(
int signatureId,
std::vector<CameraModel> & models,
StereoCameraModel & stereoModel) const
std::vector<StereoCameraModel> & stereoModels) const
{
UDEBUG("");
bool found = false;
@@ -735,7 +735,7 @@ bool DBDriver::getCalibration(
if(uContains(_trashSignatures, signatureId))
{
models = _trashSignatures.at(signatureId)->sensorData().cameraModels();
stereoModel = _trashSignatures.at(signatureId)->sensorData().stereoCameraModel();
stereoModels = _trashSignatures.at(signatureId)->sensorData().stereoCameraModels();
found = true;
}
_trashesMutex.unlock();
@@ -743,7 +743,7 @@ bool DBDriver::getCalibration(
if(!found)
{
_dbSafeAccessMutex.lock();
found = this->getCalibrationQuery(signatureId, models, stereoModel);
found = this->getCalibrationQuery(signatureId, models, stereoModels);
_dbSafeAccessMutex.unlock();
}
return found;

122
corelib/src/DBDriverSqlite3.cpp
View File

@@ -1448,7 +1448,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
cv::Mat imageCompressed;
cv::Mat depthOrRightCompressed;
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
std::vector<StereoCameraModel> stereoModels;
Transform localTransform = Transform::getIdentity();
cv::Mat scanCompressed;
cv::Mat userDataCompressed;
@@ -1515,8 +1515,16 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
}
else if(type == 1) // stereo
{
int bytesRead = (int)stereoModel.deserialize((unsigned char*)data, dataSize);
UASSERT(bytesRead == dataSize);
StereoCameraModel model;
int bytesReadTotal = 0;
unsigned int bytesRead = 0;
while(bytesReadTotal < dataSize &&
(bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
{
bytesReadTotal+=bytesRead;
stereoModels.push_back(model);
}
UASSERT(bytesReadTotal == dataSize);
}
else
{
@@ -1589,14 +1597,14 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
stereoModels.push_back(StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform,
cv::Size(dataFloat[5],dataFloat[6]));
cv::Size(dataFloat[5],dataFloat[6])));
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
@@ -1606,13 +1614,13 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
stereoModels.push_back(StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform);
localTransform));
}
else
{
@@ -1632,7 +1640,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
if(fyOrBaseline < 1.0)
{
//it is a baseline
stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
stereoModels.push_back(StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform));
}
else
{
@@ -1818,7 +1826,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
}
else
{
(*iter)->sensorData().setStereoImage(imageCompressed, depthOrRightCompressed, stereoModel);
(*iter)->sensorData().setStereoImage(imageCompressed, depthOrRightCompressed, stereoModels);
}
}
if(userData)
@@ -1850,7 +1858,7 @@ void DBDriverSqlite3::loadNodeDataQuery(std::list<Signature *> & signatures, boo
bool DBDriverSqlite3::getCalibrationQuery(
int signatureId,
std::vector<CameraModel> & models,
StereoCameraModel & stereoModel) const
std::vector<StereoCameraModel> & stereoModels) const
{
bool found = false;
if(_ppDb && signatureId)
@@ -1936,8 +1944,16 @@ bool DBDriverSqlite3::getCalibrationQuery(
}
else if(type == 1) // stereo
{
int bytesRead = (int)stereoModel.deserialize((unsigned char*)data, dataSize);
UASSERT(bytesRead == dataSize);
StereoCameraModel model;
int bytesReadTotal = 0;
unsigned int bytesRead = 0;
while(bytesReadTotal < dataSize &&
(bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
{
bytesReadTotal+=bytesRead;
stereoModels.push_back(model);
}
UASSERT(bytesReadTotal == dataSize);
}
else
{
@@ -2010,14 +2026,14 @@ bool DBDriverSqlite3::getCalibrationQuery(
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
stereoModels.push_back(StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform,
cv::Size(dataFloat[5],dataFloat[6]));
cv::Size(dataFloat[5],dataFloat[6])));
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
@@ -2027,13 +2043,13 @@ bool DBDriverSqlite3::getCalibrationQuery(
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
stereoModels.push_back((StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform);
localTransform)));
}
else
{
@@ -2054,7 +2070,7 @@ bool DBDriverSqlite3::getCalibrationQuery(
if(fyOrBaseline < 1.0)
{
//it is a baseline
stereoModel = StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform);
stereoModels.push_back(StereoCameraModel(fx,fx,cx,cy,fyOrBaseline, localTransform));
}
else
{
@@ -3278,7 +3294,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
int dataSize = 0;
Transform localTransform;
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
std::vector<StereoCameraModel> stereoModels;
// calibration
data = sqlite3_column_blob(ppStmt, index);
@@ -3308,8 +3324,16 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
}
else if(type == 1) // stereo
{
int bytesRead = (int)stereoModel.deserialize((unsigned char*)data, dataSize);
UASSERT(bytesRead == dataSize);
StereoCameraModel model;
int bytesReadTotal = 0;
unsigned int bytesRead = 0;
while(bytesReadTotal < dataSize &&
(bytesRead=model.deserialize((const unsigned char *)data+bytesReadTotal, dataSize-bytesReadTotal))!=0)
{
bytesReadTotal+=bytesRead;
stereoModels.push_back(model);
}
UASSERT(bytesReadTotal == dataSize);
}
else
{
@@ -3383,14 +3407,14 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
stereoModels.push_back(StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform,
cv::Size(dataFloat[5], dataFloat[6]));
cv::Size(dataFloat[5], dataFloat[6])));
}
else if((unsigned int)dataSize == (5+localTransform.size())*sizeof(float))
{
@@ -3400,13 +3424,13 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
{
localTransform.normalizeRotation();
}
stereoModel = StereoCameraModel(
stereoModels.push_back(StereoCameraModel(
dataFloat[0], // fx
dataFloat[1], // fy
dataFloat[2], // cx
dataFloat[3], // cy
dataFloat[4], // baseline
localTransform);
localTransform));
}
else
{
@@ -3415,7 +3439,7 @@ void DBDriverSqlite3::loadSignaturesQuery(const std::list<int> & ids, std::list<
}
(*iter)->sensorData().setCameraModels(models);
(*iter)->sensorData().setStereoCameraModel(stereoModel);
(*iter)->sensorData().setStereoCameraModels(stereoModels);
}
rc = sqlite3_step(ppStmt);
}
@@ -4382,8 +4406,8 @@ void DBDriverSqlite3::saveQuery(const std::list<Signature *> & signatures)
!(*i)->sensorData().depthOrRightCompressed().empty() ||
!(*i)->sensorData().laserScanCompressed().isEmpty() ||
!(*i)->sensorData().userDataCompressed().empty() ||
!(*i)->sensorData().cameraModels().size() ||
!(*i)->sensorData().stereoCameraModel().isValidForProjection())
!(*i)->sensorData().cameraModels().empty() ||
!(*i)->sensorData().stereoCameraModels().empty())
{
UASSERT((*i)->id() == (*i)->sensorData().id());
stepSensorData(ppStmt, (*i)->sensorData());
@@ -5691,13 +5715,15 @@ void DBDriverSqlite3::stepDepth(sqlite3_stmt * ppStmt, const SensorData & sensor
cy = sensorData.cameraModels()[0].cy();
localTransform = sensorData.cameraModels()[0].localTransform();
}
else if(sensorData.stereoCameraModel().isValidForProjection())
else if(sensorData.stereoCameraModels().size())
{
fx = sensorData.stereoCameraModel().left().fx();
fyOrBaseline = sensorData.stereoCameraModel().baseline();
cx = sensorData.stereoCameraModel().left().cx();
cy = sensorData.stereoCameraModel().left().cy();
localTransform = sensorData.stereoCameraModel().left().localTransform();
UASSERT_MSG(sensorData.stereoCameraModels().size() == 1,
uFormat("Database version %s doesn't support multi-camera!", _version.c_str()).c_str());
fx = sensorData.stereoCameraModels()[0].left().fx();
fyOrBaseline = sensorData.stereoCameraModels()[0].baseline();
cx = sensorData.stereoCameraModels()[0].left().cx();
cy = sensorData.stereoCameraModels()[0].left().cy();
localTransform = sensorData.stereoCameraModels()[0].left().localTransform();
}
if(uStrNumCmp(_version, "0.7.0") >= 0)
@@ -6040,24 +6066,32 @@ void DBDriverSqlite3::stepSensorData(sqlite3_stmt * ppStmt,
}
}
}
else if(sensorData.stereoCameraModel().isValidForProjection())
else if(sensorData.stereoCameraModels().size() && sensorData.stereoCameraModels()[0].isValidForProjection())
{
if(uStrNumCmp(_version, "0.18.0") >= 0)
{
calibrationData = sensorData.stereoCameraModel().serialize();
UASSERT(!calibrationData.empty());
for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
{
UASSERT(sensorData.stereoCameraModels()[i].isValidForProjection());
std::vector<unsigned char> data = sensorData.stereoCameraModels()[i].serialize();
UASSERT(!data.empty());
unsigned int oldSize = calibrationData.size();
calibrationData.resize(calibrationData.size() + data.size());
memcpy(calibrationData.data()+oldSize, data.data(), data.size());
}
}
else
{
const Transform & localTransform = sensorData.stereoCameraModel().left().localTransform();
UASSERT_MSG(sensorData.stereoCameraModels().size()==1, uFormat("Database version (%s) is too old for saving multiple stereo cameras", _version.c_str()).c_str());
const Transform & localTransform = sensorData.stereoCameraModels()[0].left().localTransform();
calibration.resize(7+localTransform.size());
calibration[0] = sensorData.stereoCameraModel().left().fx();
calibration[1] = sensorData.stereoCameraModel().left().fy();
calibration[2] = sensorData.stereoCameraModel().left().cx();
calibration[3] = sensorData.stereoCameraModel().left().cy();
calibration[4] = sensorData.stereoCameraModel().baseline();
calibration[5] = sensorData.stereoCameraModel().left().imageWidth();
calibration[6] = sensorData.stereoCameraModel().left().imageHeight();
calibration[0] = sensorData.stereoCameraModels()[0].left().fx();
calibration[1] = sensorData.stereoCameraModels()[0].left().fy();
calibration[2] = sensorData.stereoCameraModels()[0].left().cx();
calibration[3] = sensorData.stereoCameraModels()[0].left().cy();
calibration[4] = sensorData.stereoCameraModels()[0].baseline();
calibration[5] = sensorData.stereoCameraModels()[0].left().imageWidth();
calibration[6] = sensorData.stereoCameraModels()[0].left().imageHeight();
memcpy(calibration.data()+7, localTransform.data(), localTransform.size()*sizeof(float));
}
}

6
corelib/src/DBReader.cpp
View File

@@ -186,8 +186,8 @@ bool DBReader::init(
if(_ids.size())
{
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModel))
std::vector<StereoCameraModel> stereoModels;
if(_dbDriver->getCalibration(*_ids.begin(), models, stereoModels))
{
if(models.size())
{
@@ -208,7 +208,7 @@ bool DBReader::init(
}
}
}
else if(stereoModel.isValidForProjection())
else if(stereoModels.size() && stereoModels.at(0).isValidForProjection())
{
_calibrated = true;
}

129
corelib/src/Features2d.cpp
View File

@@ -792,7 +792,9 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
std::vector<cv::Point3f> keypoints3D;
if(keypoints.size())
{
if(!data.rightRaw().empty() && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection())
if(!data.rightRaw().empty() && !data.imageRaw().empty() &&
!data.stereoCameraModels().empty() &&
data.stereoCameraModels()[0].isValidForProjection())
{
//stereo
cv::Mat imageMono;
@@ -808,22 +810,121 @@ std::vector<cv::Point3f> Feature2D::generateKeypoints3D(
std::vector<cv::Point2f> leftCorners;
cv::KeyPoint::convert(keypoints, leftCorners);
std::vector<unsigned char> status;
std::vector<cv::Point2f> rightCorners;
rightCorners = _stereo->computeCorrespondences(
imageMono,
data.rightRaw(),
leftCorners,
status);
keypoints3D = util3d::generateKeypoints3DStereo(
leftCorners,
rightCorners,
data.stereoCameraModel(),
status,
_minDepth,
_maxDepth);
if(data.stereoCameraModels().size() == 1)
{
std::vector<unsigned char> status;
rightCorners = _stereo->computeCorrespondences(
imageMono,
data.rightRaw(),
leftCorners,
status);
if(ULogger::level() >= ULogger::kWarning)
{
int rejected = 0;
for(size_t i=0; i<status.size(); ++i)
{
if(status[i]==0)
{
++rejected;
}
}
if(rejected > (int)status.size()/2)
{
UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
"Optical flow may have failed because images are not calibrated, "
"the background is too far (no disparity between the images), "
"maximum disparity may be too small (%f) or that exposure between "
"left and right images is too different.",
rejected,
(int)status.size(),
_stereo->maxDisparity());
}
}
keypoints3D = util3d::generateKeypoints3DStereo(
leftCorners,
rightCorners,
data.stereoCameraModels()[0],
status,
_minDepth,
_maxDepth);
}
else
{
int subImageWith = imageMono.cols / data.stereoCameraModels().size();
UASSERT(imageMono.cols % subImageWith == 0);
std::vector<std::vector<cv::Point2f> > subLeftCorners(data.stereoCameraModels().size());
std::vector<std::vector<int> > subIndex(data.stereoCameraModels().size());
// Assign keypoints per camera
for(size_t i=0; i<leftCorners.size(); ++i)
{
int cameraIndex = int(leftCorners[i].x / subImageWith);
leftCorners[i].x -= cameraIndex*subImageWith;
subLeftCorners[cameraIndex].push_back(leftCorners[i]);
subIndex[cameraIndex].push_back(i);
}
keypoints3D.resize(keypoints.size());
int total = 0;
int rejected = 0;
for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
{
if(!subLeftCorners[i].empty())
{
std::vector<unsigned char> status;
rightCorners = _stereo->computeCorrespondences(
imageMono.colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
data.rightRaw().colRange(cv::Range(subImageWith*i, subImageWith*(i+1))),
subLeftCorners[i],
status);
std::vector<cv::Point3f> subKeypoints3D = util3d::generateKeypoints3DStereo(
subLeftCorners[i],
rightCorners,
data.stereoCameraModels()[i],
status,
_minDepth,
_maxDepth);
if(ULogger::level() >= ULogger::kWarning)
{
for(size_t i=0; i<status.size(); ++i)
{
if(status[i]==0)
{
++rejected;
}
}
total+=status.size();
}
UASSERT(subIndex[i].size() == subKeypoints3D.size());
for(size_t j=0; j<subKeypoints3D.size(); ++j)
{
keypoints3D[subIndex[i][j]] = subKeypoints3D[j];
}
}
}
if(ULogger::level() >= ULogger::kWarning)
{
if(rejected > total/2)
{
UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
"Optical flow may have failed because images are not calibrated, "
"the background is too far (no disparity between the images), "
"maximum disparity may be too small (%f) or that exposure between "
"left and right images is too different.",
rejected,
total,
_stereo->maxDisparity());
}
}
}
}
else if(!data.depthRaw().empty() && data.cameraModels().size())
{

178
corelib/src/Memory.cpp
View File

@@ -1812,7 +1812,7 @@ void Memory::clear()
_linksChanged = false;
_gpsOrigin = GPS();
_rectCameraModels.clear();
_rectStereoCameraModel = StereoCameraModel();
_rectStereoCameraModels.clear();
_odomMaxInf.clear();
_groundTruths.clear();
_labels.clear();
@@ -2963,7 +2963,7 @@ Transform Memory::computeTransform(
}
std::map<int, Transform> bundlePoses;
std::multimap<int, Link> bundleLinks;
std::map<int, CameraModel> bundleModels;
std::map<int, std::vector<CameraModel> > bundleModels;
std::map<int, std::map<int, FeatureBA> > wordReferences;
std::multimap<int, Link> links = fromS.getLinks();
@@ -2989,28 +2989,32 @@ Transform Memory::computeTransform(
}
if(s)
{
CameraModel model;
if(s->sensorData().cameraModels().size() == 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
std::vector<CameraModel> models;
if(s->sensorData().cameraModels().size() >= 1 && s->sensorData().cameraModels().at(0).isValidForProjection())
{
model = s->sensorData().cameraModels()[0];
models = s->sensorData().cameraModels();
}
else if(s->sensorData().stereoCameraModel().isValidForProjection())
else if(s->sensorData().stereoCameraModels().size() >= 1 && s->sensorData().stereoCameraModels().at(0).isValidForProjection())
{
model = s->sensorData().stereoCameraModel().left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-s->sensorData().stereoCameraModel().baseline()*model.fx());
for(size_t i=0; i<s->sensorData().stereoCameraModels().size(); ++i)
{
CameraModel model = s->sensorData().stereoCameraModels()[i].left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-s->sensorData().stereoCameraModels()[i].baseline()*model.fx(),
model.imageSize());
models.push_back(model);
}
}
else
{
UFATAL("no valid camera model to use local bundle adjustment on loop closure!");
}
bundleModels.insert(std::make_pair(id, model));
Transform invLocalTransform = model.localTransform().inverse();
bundleModels.insert(std::make_pair(id, models));
UASSERT(iter->second.isValid() || iter->first == fromS.id());
if(iter->second.transform().isNull())
@@ -3030,16 +3034,27 @@ Transform Memory::computeTransform(
if(points3DMap.find(jter->first)!=points3DMap.end() &&
(id == tmpTo.id() || jter->first > 0)) // Since we added negative words of "from", only accept matches with current frame
{
cv::KeyPoint kpts = s->getWordsKpts()[jter->second];
int cameraIndex = 0;
if(models.size()>1)
{
UASSERT(models[0].imageWidth()>0);
float subImageWidth = models[0].imageWidth();
cameraIndex = int(kpts.pt.x / subImageWidth);
kpts.pt.x = kpts.pt.x - (subImageWidth*float(cameraIndex));
}
//get depth
float d = 0.0f;
if( !s->getWords3().empty() &&
util3d::isFinite(s->getWords3()[jter->second]))
{
//move back point in camera frame (to get depth along z)
Transform invLocalTransform = models[cameraIndex].localTransform().inverse();
d = util3d::transformPoint(s->getWords3()[jter->second], invLocalTransform).z;
}
wordReferences.insert(std::make_pair(jter->first, std::map<int, FeatureBA>()));
wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(s->getWordsKpts()[jter->second], d)));
wordReferences.at(jter->first).insert(std::make_pair(id, FeatureBA(kpts, d, cv::Mat(), cameraIndex)));
++totalWordReferences;
}
}
@@ -4110,19 +4125,19 @@ void Memory::getNodeWordsAndGlobalDescriptors(int nodeId,
void Memory::getNodeCalibration(int nodeId,
std::vector<CameraModel> & models,
StereoCameraModel & stereoModel) const
std::vector<StereoCameraModel> & stereoModels) const
{
//UDEBUG("nodeId=%d", nodeId);
Signature * s = this->_getSignature(nodeId);
if(s)
{
models = s->sensorData().cameraModels();
stereoModel = s->sensorData().stereoCameraModel();
stereoModels = s->sensorData().stereoCameraModels();
}
else if(_dbDriver)
{
// load from database
_dbDriver->getCalibration(nodeId, models, stereoModel);
_dbDriver->getCalibration(nodeId, models, stereoModels);
}
}
@@ -4445,15 +4460,11 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
CV_16UC1, CV_32FC1, CV_8UC1).c_str());
if(!data.depthOrRightRaw().empty() &&
data.cameraModels().size() == 0 &&
!data.stereoCameraModel().isValidForProjection() &&
data.cameraModels().empty() &&
data.stereoCameraModels().empty() &&
!pose.isNull())
{
UERROR("Camera calibration not valid, calibrate your camera!");
if(data.cameraModels().empty())
std::cout << data.stereoCameraModel() << std::endl;
else
std::cout << data.cameraModels()[0] << std::endl;
UERROR("No camera calibration found, calibrate your camera!");
return 0;
}
UASSERT(_feature2D != 0);
@@ -4504,6 +4515,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
// we assume that once rtabmap is receiving data, the calibration won't change over time
if(data.cameraModels().size())
{
UDEBUG("Monocular rectification");
// Note that only RGB image is rectified, the depth image is assumed to be already registered to rectified RGB camera.
UASSERT(int((data.imageRaw().cols/data.cameraModels().size())*data.cameraModels().size()) == data.imageRaw().cols);
int subImageWidth = data.imageRaw().cols/data.cameraModels().size();
@@ -4545,25 +4557,58 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
data.setRGBDImage(rectifiedImages, data.depthOrRightRaw(), data.cameraModels());
}
else if(data.stereoCameraModel().isValidForRectification())
else if(data.stereoCameraModels().size())
{
if(!_rectStereoCameraModel.isValidForRectification())
UDEBUG("Stereo rectification");
UASSERT(int((data.imageRaw().cols/data.stereoCameraModels().size())*data.stereoCameraModels().size()) == data.imageRaw().cols);
int subImageWidth = data.imageRaw().cols/data.stereoCameraModels().size();
UASSERT(subImageWidth == data.rightRaw().cols/(int)data.stereoCameraModels().size());
cv::Mat rectifiedLefts(data.imageRaw().size(), data.imageRaw().type());
cv::Mat rectifiedRights(data.rightRaw().size(), data.rightRaw().type());
bool initRectMaps = _rectStereoCameraModels.empty();
if(initRectMaps)
{
_rectStereoCameraModel = data.stereoCameraModel();
if(!_rectStereoCameraModel.isRectificationMapInitialized())
_rectStereoCameraModels.resize(data.stereoCameraModels().size());
}
for(unsigned int i=0; i<data.stereoCameraModels().size(); ++i)
{
if(data.stereoCameraModels()[i].isValidForRectification())
{
UWARN("Initializing rectification maps (only done for the first image received)...");
_rectStereoCameraModel.initRectificationMap();
UWARN("Initializing rectification maps (only done for the first image received)...done!");
if(initRectMaps)
{
_rectStereoCameraModels[i] = data.stereoCameraModels()[i];
if(!_rectStereoCameraModels[i].isRectificationMapInitialized())
{
UWARN("Initializing rectification maps (only done for the first image received)...");
_rectStereoCameraModels[i].initRectificationMap();
UWARN("Initializing rectification maps (only done for the first image received)...done!");
}
}
UASSERT(_rectStereoCameraModels[i].left().imageWidth() == data.stereoCameraModels()[i].left().imageWidth());
UASSERT(_rectStereoCameraModels[i].left().imageHeight() == data.stereoCameraModels()[i].left().imageHeight());
cv::Mat rectifiedLeft = _rectStereoCameraModels[i].left().rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
cv::Mat rectifiedRight = _rectStereoCameraModels[i].right().rectifyImage(cv::Mat(data.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.rightRaw().rows)));
rectifiedLeft.copyTo(cv::Mat(rectifiedLefts, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
rectifiedRight.copyTo(cv::Mat(rectifiedRights, cv::Rect(subImageWidth*i, 0, subImageWidth, data.rightRaw().rows)));
imagesRectified = true;
}
else
{
UERROR("Calibration for camera %d cannot be used to rectify the image. Make sure to do a "
"full calibration. If images are already rectified, set %s parameter back to true.",
(int)i,
Parameters::kRtabmapImagesAlreadyRectified().c_str());
std::cout << data.stereoCameraModels()[i] << std::endl;
return 0;
}
}
UASSERT(_rectStereoCameraModel.left().imageWidth() == data.stereoCameraModel().left().imageWidth());
UASSERT(_rectStereoCameraModel.left().imageHeight() == data.stereoCameraModel().left().imageHeight());
data.setStereoImage(
_rectStereoCameraModel.left().rectifyImage(data.imageRaw()),
_rectStereoCameraModel.right().rectifyImage(data.rightRaw()),
data.stereoCameraModel());
imagesRectified = true;
rectifiedLefts,
rectifiedRights,
data.stereoCameraModels());
}
else
{
@@ -4636,17 +4681,18 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
util2d::decimate(decimatedData.depthOrRightRaw(), decimationDepth),
cameraModels);
}
else
std::vector<StereoCameraModel> stereoCameraModels = decimatedData.stereoCameraModels();
for(unsigned int i=0; i<stereoCameraModels.size(); ++i)
{
stereoCameraModels[i].scale(1.0/double(_imagePreDecimation));
}
if(!stereoCameraModels.empty())
{
StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
if(stereoModel.isValidForProjection())
{
stereoModel.scale(1.0/double(_imagePreDecimation));
}
decimatedData.setStereoImage(
util2d::decimate(decimatedData.imageRaw(), _imagePreDecimation),
util2d::decimate(decimatedData.depthOrRightRaw(), _imagePreDecimation),
stereoModel);
stereoCameraModels);
}
}
@@ -4885,7 +4931,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
keypoints3D = data.keypoints3D();
}
else if((!decimatedData.depthRaw().empty() && decimatedData.cameraModels().size() && decimatedData.cameraModels()[0].isValidForProjection()) ||
(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModel().isValidForProjection()))
(!decimatedData.rightRaw().empty() && decimatedData.stereoCameraModels().size() && decimatedData.stereoCameraModels()[0].isValidForProjection()))
{
keypoints3D = _feature2D->generateKeypoints3D(decimatedData, keypoints);
t = timer.ticks();
@@ -5090,7 +5136,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
if(keypoints3D.empty() &&
((!data.depthRaw().empty() && data.cameraModels().size() && data.cameraModels()[0].isValidForProjection()) ||
(!data.rightRaw().empty() && data.stereoCameraModel().isValidForProjection())))
(!data.rightRaw().empty() && data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection())))
{
keypoints3D = _feature2D->generateKeypoints3D(data, keypoints);
}
@@ -5280,9 +5326,21 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
markers = _markerDetector->detect(data.imageRaw(), data.cameraModels()[0], data.depthRaw(), _landmarksSize);
}
}
else if(data.stereoCameraModel().isValidForProjection())
else if(!data.stereoCameraModels().empty() && data.stereoCameraModels()[0].isValidForProjection())
{
markers = _markerDetector->detect(data.imageRaw(), data.stereoCameraModel().left(), cv::Mat(), _landmarksSize);
if(data.stereoCameraModels().size() > 1)
{
static bool warned = false;
if(!warned)
{
UWARN("Detecting markers in multi-camera setup is not yet implemented, aborting marker detection. This message is only printed once.");
}
warned = true;
}
else
{
markers = _markerDetector->detect(data.imageRaw(), data.stereoCameraModels()[0].left(), cv::Mat(), _landmarksSize);
}
}
for(std::map<int, MarkerInfo>::iterator iter=markers.begin(); iter!=markers.end(); ++iter)
{
@@ -5310,7 +5368,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
cv::Mat image = data.imageRaw();
cv::Mat depthOrRightImage = data.depthOrRightRaw();
std::vector<CameraModel> cameraModels = data.cameraModels();
StereoCameraModel stereoCameraModel = data.stereoCameraModel();
std::vector<StereoCameraModel> stereoCameraModels = data.stereoCameraModels();
// apply decimation?
if(_imagePostDecimation > 1 && !isIntermediateNode)
@@ -5320,7 +5378,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
image = decimatedData.imageRaw();
depthOrRightImage = decimatedData.depthOrRightRaw();
cameraModels = decimatedData.cameraModels();
stereoCameraModel = decimatedData.stereoCameraModel();
stereoCameraModels = decimatedData.stereoCameraModels();
}
else
{
@@ -5348,9 +5406,9 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
{
cameraModels[i] = cameraModels[i].scaled(1.0/double(_imagePostDecimation));
}
if(stereoCameraModel.isValidForProjection())
for(unsigned int i=0; i<stereoCameraModels.size(); ++i)
{
stereoCameraModel.scale(1.0/double(_imagePostDecimation));
stereoCameraModels[i].scale(1.0/double(_imagePostDecimation));
}
}
@@ -5598,7 +5656,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
"",
pose,
data.groundTruth(),
stereoCameraModel.isValidForProjection()?
!stereoCameraModels.empty()?
SensorData(
laserScan.angleIncrement() == 0.0f?
LaserScan(compressedScan,
@@ -5616,7 +5674,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
laserScan.localTransform()),
compressedImage,
compressedDepth,
stereoCameraModel,
stereoCameraModels,
id,
0,
compressedUserData):
@@ -5682,7 +5740,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
"",
pose,
data.groundTruth(),
stereoCameraModel.isValidForProjection()?
!stereoCameraModels.empty()?
SensorData(
laserScan.angleIncrement() == 0.0f?
LaserScan(compressedScan,
@@ -5700,7 +5758,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
laserScan.localTransform()),
cv::Mat(),
cv::Mat(),
stereoCameraModel,
stereoCameraModels,
id,
0,
compressedUserData):
@@ -5738,7 +5796,7 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
}
else
{
s->sensorData().setStereoImage(image, depthOrRightImage, stereoCameraModel, false);
s->sensorData().setStereoImage(image, depthOrRightImage, stereoCameraModels, false);
}
s->sensorData().setLaserScan(laserScan, false);
s->sensorData().setUserData(data.userDataRaw(), false);

21
corelib/src/OccupancyGrid.cpp
View File

@@ -430,8 +430,25 @@ void OccupancyGrid::createLocalMap(
}
else
{
const Transform & t = node.sensorData().stereoCameraModel().localTransform();
viewPoint = cv::Point3f(t.x(), t.y(), t.z());
// average of all local transforms
float sum = 0;
for(unsigned int i=0; i<node.sensorData().stereoCameraModels().size(); ++i)
{
const Transform & t = node.sensorData().stereoCameraModels()[i].localTransform();
if(!t.isNull())
{
viewPoint.x += t.x();
viewPoint.y += t.y();
viewPoint.z += t.z();
sum += 1.0f;
}
}
if(sum > 0.0f)
{
viewPoint.x /= sum;
viewPoint.y /= sum;
viewPoint.z /= sum;
}
}
cv::Mat scanGroundCells;

81
corelib/src/Odometry.cpp
View File

@@ -328,35 +328,73 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
if(!_imagesAlreadyRectified && !this->canProcessRawImages() && !data.imageRaw().empty())
{
if(data.stereoCameraModel().isValidForRectification())
if(!data.stereoCameraModels().empty())
{
if(!stereoModel_.isRectificationMapInitialized() ||
stereoModel_.left().imageSize() != data.stereoCameraModel().left().imageSize())
bool valid = true;
if(data.stereoCameraModels().size() != stereoModels_.size())
{
stereoModel_ = data.stereoCameraModel();
stereoModel_.initRectificationMap();
if(stereoModel_.isRectificationMapInitialized())
stereoModels_.clear();
valid = false;
}
else
{
for(size_t i=0; i<data.stereoCameraModels().size() && valid; ++i)
{
valid = stereoModels_[i].isRectificationMapInitialized() &&
stereoModels_[i].left().imageSize() == data.stereoCameraModels()[i].left().imageSize();
}
}
if(!valid)
{
stereoModels_ = data.stereoCameraModels();
valid = true;
for(size_t i=0; i<stereoModels_.size() && valid; ++i)
{
stereoModels_[i].initRectificationMap();
valid = stereoModels_[i].isRectificationMapInitialized();
}
if(valid)
{
UWARN("%s parameter is set to false but the selected odometry approach cannot "
"process raw images. We will rectify them for convenience.",
"process raw stereo images. We will rectify them for convenience.",
Parameters::kRtabmapImagesAlreadyRectified().c_str());
}
else
{
UERROR("Odometry approach chosen cannot process raw images (not rectified images) "
UERROR("Odometry approach chosen cannot process raw stereo images (not rectified images) "
"and we cannot rectify them as the rectification map failed to initialize (valid calibration?). "
"Make sure images are rectified and set %s parameter back to true, or make sure "
"calibration is valid for rectification.",
"Make sure images are rectified and set %s parameter back to true, or "
"make sure calibration is valid for rectification",
Parameters::kRtabmapImagesAlreadyRectified().c_str());
stereoModels_.clear();
}
}
if(stereoModel_.isRectificationMapInitialized())
if(valid)
{
data.setStereoImage(
stereoModel_.left().rectifyImage(data.imageRaw()),
stereoModel_.right().rectifyImage(data.rightRaw()),
stereoModel_,
false);
if(stereoModels_.size()==1)
{
data.setStereoImage(
stereoModels_[0].left().rectifyImage(data.imageRaw()),
stereoModels_[0].right().rectifyImage(data.rightRaw()),
stereoModels_,
false);
}
else
{
UASSERT(int((data.imageRaw().cols/data.stereoCameraModels().size())*data.stereoCameraModels().size()) == data.imageRaw().cols);
int subImageWidth = data.imageRaw().cols/data.stereoCameraModels().size();
cv::Mat rectifiedLeftImages = data.imageRaw().clone();
cv::Mat rectifiedRightImages = data.imageRaw().clone();
for(size_t i=0; i<stereoModels_.size() && valid; ++i)
{
cv::Mat rectifiedLeft = stereoModels_[i].left().rectifyImage(cv::Mat(data.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
cv::Mat rectifiedRight = stereoModels_[i].right().rectifyImage(cv::Mat(data.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data.rightRaw().rows)));
rectifiedLeft.copyTo(cv::Mat(rectifiedLeftImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
rectifiedRight.copyTo(cv::Mat(rectifiedRightImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data.imageRaw().rows)));
}
data.setStereoImage(rectifiedLeftImages, rectifiedRightImages, stereoModels_, false);
}
}
}
else if(!data.cameraModels().empty())
@@ -599,12 +637,15 @@ Transform Odometry::process(SensorData & data, const Transform & guessIn, Odomet
}
else
{
StereoCameraModel stereoModel = decimatedData.stereoCameraModel();
if(stereoModel.isValidForProjection())
std::vector<StereoCameraModel> stereoModels = decimatedData.stereoCameraModels();
for(unsigned int i=0; i<stereoModels.size(); ++i)
{
stereoModel.scale(1.0/double(_imageDecimation));
stereoModels[i].scale(1.0/double(_imageDecimation));
}
if(!stereoModels.empty())
{
decimatedData.setStereoImage(rgbLeft, depthRight, stereoModels);
}
decimatedData.setStereoImage(rgbLeft, depthRight, stereoModel);
}

4
corelib/src/OdometryThread.cpp
View File

@@ -134,7 +134,7 @@ void OdometryThread::addData(const SensorData & data)
{
if(dynamic_cast<OdometryMono*>(_odometry) == 0)
{
if((data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection())) &&
if((data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty())) &&
data.laserScanRaw().empty())
{
ULOGGER_ERROR("Missing some information (images/scans empty or missing calibration)!?");
@@ -144,7 +144,7 @@ void OdometryThread::addData(const SensorData & data)
else
{
// Mono can accept RGB only
if(data.imageRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
if(data.imageRaw().empty() || (data.cameraModels().empty() && data.stereoCameraModels().empty()))
{
ULOGGER_ERROR("Missing some information (image empty or missing calibration)!?");
return;

120
corelib/src/Optimizer.cpp
View File

@@ -438,7 +438,7 @@ std::map<int, Transform> Optimizer::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models,
const std::map<int, std::vector<CameraModel> > & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences,
std::set<int> * outliers)
@@ -457,37 +457,35 @@ std::map<int, Transform> Optimizer::optimizeBA(
bool rematchFeatures)
{
UDEBUG("");
std::map<int, CameraModel> models;
std::map<int, std::vector<CameraModel> > multiModels;
std::map<int, Transform> poses;
for(std::map<int, Transform>::const_iterator iter=posesIn.lower_bound(1); iter!=posesIn.end(); ++iter)
{
// Get camera model
CameraModel model;
std::vector<CameraModel> models;
if(uContains(signatures, iter->first))
{
if(signatures.at(iter->first).sensorData().cameraModels().size() == 1 && signatures.at(iter->first).sensorData().cameraModels().at(0).isValidForProjection())
const SensorData & s = signatures.at(iter->first).sensorData();
if(s.cameraModels().size() >= 1 && s.cameraModels().at(0).isValidForProjection())
{
model = signatures.at(iter->first).sensorData().cameraModels()[0];
models = s.cameraModels();
}
else if(signatures.at(iter->first).sensorData().stereoCameraModel().isValidForProjection())
else if(!s.stereoCameraModels().empty() && s.stereoCameraModels()[0].isValidForProjection())
{
model = signatures.at(iter->first).sensorData().stereoCameraModel().left();
for(size_t i=0; i<s.stereoCameraModels().size(); ++i)
{
CameraModel model = s.stereoCameraModels()[i].left();
// Set Tx = -baseline*fx for stereo BA
model = CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-signatures.at(iter->first).sensorData().stereoCameraModel().baseline()*model.fx());
}
else if(signatures.at(iter->first).sensorData().cameraModels().size() > 1)
{
UERROR("Multi-cameras (%d) is not supported (id=%d).",
signatures.at(iter->first).sensorData().cameraModels().size(),
iter->first);
return std::map<int, Transform>();
// Set Tx = -baseline*fx for stereo BA
models.push_back(CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-s.stereoCameraModels()[i].baseline()*model.fx(),
model.imageSize()));
}
}
else
{
@@ -501,16 +499,14 @@ std::map<int, Transform> Optimizer::optimizeBA(
return std::map<int, Transform>();
}
UASSERT(model.isValidForProjection());
models.insert(std::make_pair(iter->first, model));
multiModels.insert(std::make_pair(iter->first, models));
poses.insert(*iter);
}
// compute correspondences
this->computeBACorrespondences(poses, links, signatures, points3DMap, wordReferences, rematchFeatures);
return optimizeBA(rootId, poses, links, models, points3DMap, wordReferences);
return optimizeBA(rootId, poses, links, multiModels, points3DMap, wordReferences);
}
std::map<int, Transform> Optimizer::optimizeBA(
@@ -537,9 +533,11 @@ Transform Optimizer::optimizeBA(
poses.insert(std::make_pair(link.to(), link.transform()));
std::multimap<int, Link> links;
links.insert(std::make_pair(link.from(), link));
std::map<int, CameraModel> models;
models.insert(std::make_pair(link.from(), model));
models.insert(std::make_pair(link.to(), model));
std::map<int, std::vector<CameraModel> > models;
std::vector<CameraModel> tmp;
tmp.push_back(model);
models.insert(std::make_pair(link.from(), tmp));
models.insert(std::make_pair(link.to(), tmp));
poses = optimizeBA(link.from(), poses, links, models, points3DMap, wordReferences, outliers);
if(poses.size() == 2)
{
@@ -567,7 +565,7 @@ void Optimizer::computeBACorrespondences(
std::map<int, std::map<int, FeatureBA> > & wordReferences,
bool rematchFeatures)
{
UDEBUG("");
UDEBUG("rematchFeatures=%d", rematchFeatures?1:0);
int wordCount = 0;
int edgeWithWordsAdded = 0;
std::map<int, std::map<cv::KeyPoint, int, KeyPointCompare> > frameToWordMap; // <FrameId, <Keypoint, wordId> >
@@ -587,6 +585,14 @@ void Optimizer::computeBACorrespondences(
if(sFrom.getWeight() >= 0) // ignore intermediate links
{
Signature sTo = signatures.at(link.to());
if((sFrom.sensorData().cameraModels().empty() && sFrom.sensorData().stereoCameraModels().empty()) ||
(sTo.sensorData().cameraModels().empty() && sTo.sensorData().stereoCameraModels().empty()))
{
UERROR("No camera models found");
continue;
}
if(sTo.getWeight() < 0)
{
for(std::multimap<int, Link>::const_iterator jter=links.find(sTo.id());
@@ -675,8 +681,7 @@ void Optimizer::computeBACorrespondences(
wordId = ++wordCount;
wordReferences.insert(std::make_pair(wordId, std::map<int, FeatureBA>()));
p = util3d::transformPoint(p, pose);
points3DMap.insert(std::make_pair(wordId, p));
points3DMap.insert(std::make_pair(wordId, util3d::transformPoint(p, pose)));
}
else
{
@@ -692,7 +697,27 @@ void Optimizer::computeBACorrespondences(
UASSERT(indexFrom < sFrom.getWordsDescriptors().rows);
descriptorFrom = sFrom.getWordsDescriptors().row(indexFrom);
}
wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, p.x, descriptorFrom)));
int cameraIndex = 0;
if(sFrom.sensorData().cameraModels().size()>1 || sFrom.sensorData().stereoCameraModels().size()>1)
{
float subImageWidth = sFrom.sensorData().cameraModels().size()>1?sFrom.sensorData().cameraModels()[0].imageWidth():sFrom.sensorData().stereoCameraModels()[0].left().imageWidth();
cameraIndex = int(ptFrom.pt.x / subImageWidth);
ptFrom.pt.x = ptFrom.pt.x - (subImageWidth*float(cameraIndex));
}
float depth = 0.0f;
if(!sFrom.sensorData().cameraModels().empty())
{
depth = util3d::transformPoint(p, sFrom.sensorData().cameraModels()[cameraIndex].localTransform().inverse()).z;
}
else
{
UASSERT(!sFrom.sensorData().stereoCameraModels().empty());
depth = util3d::transformPoint(p, sFrom.sensorData().stereoCameraModels()[cameraIndex].localTransform().inverse()).z;
}
wordReferences.at(wordId).insert(std::make_pair(sFrom.id(), FeatureBA(ptFrom, depth, descriptorFrom, cameraIndex)));
frameToWordMap.insert(std::make_pair(sFrom.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
frameToWordMap.at(sFrom.id()).insert(std::make_pair(ptFrom, wordId));
}
@@ -705,17 +730,32 @@ void Optimizer::computeBACorrespondences(
UASSERT(indexTo < sTo.getWordsDescriptors().rows);
descriptorTo = sTo.getWordsDescriptors().row(indexTo);
}
int cameraIndex = 0;
if(sTo.sensorData().cameraModels().size()>1 || sTo.sensorData().stereoCameraModels().size()>1)
{
float subImageWidth = sTo.sensorData().cameraModels().size()>1?sTo.sensorData().cameraModels()[0].imageWidth():sTo.sensorData().stereoCameraModels()[0].left().imageWidth();
cameraIndex = int(ptTo.pt.x / subImageWidth);
ptTo.pt.x = ptTo.pt.x - (subImageWidth*float(cameraIndex));
}
float depth = 0.0f;
if(!sTo.getWords3().empty())
{
UASSERT(indexTo < (int)sTo.getWords3().size());
const cv::Point3f & pt = sTo.getWords3()[indexTo];
if( pt.x > 0)
if(!sTo.sensorData().cameraModels().empty())
{
depth = pt.x;
depth = util3d::transformPoint(pt, sTo.sensorData().cameraModels()[cameraIndex].localTransform().inverse()).z;
}
else
{
UASSERT(!sTo.sensorData().stereoCameraModels().empty());
depth = util3d::transformPoint(pt, sTo.sensorData().stereoCameraModels()[cameraIndex].localTransform().inverse()).z;
}
}
wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo)));
wordReferences.at(wordId).insert(std::make_pair(sTo.id(), FeatureBA(ptTo, depth, descriptorTo, cameraIndex)));
frameToWordMap.insert(std::make_pair(sTo.id(), std::map<cv::KeyPoint, int, KeyPointCompare>()));
frameToWordMap.at(sTo.id()).insert(std::make_pair(ptTo, wordId));
}
@@ -732,6 +772,14 @@ void Optimizer::computeBACorrespondences(
}
}
UDEBUG("Added %d words (edges with words=%d/%d)", wordCount, edgeWithWordsAdded, links.size());
if(links.empty())
{
UERROR("No links found for BA?!");
}
else if(wordCount == 0)
{
UERROR("No words added for BA?!");
}
}
} /* namespace rtabmap */

6
corelib/src/Parameters.cpp
View File

@@ -661,6 +661,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With OpenGV:";
#ifdef RTABMAP_OPENGV
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With Madgwick:";
#ifdef RTABMAP_MADGWICK

410
corelib/src/RegistrationVis.cpp
View File

@@ -310,7 +310,7 @@ Transform RegistrationVis::computeTransformationImpl(
fromSignature.sensorData().imageRaw().cols,
fromSignature.sensorData().imageRaw().rows,
(int)fromSignature.sensorData().cameraModels().size(),
fromSignature.sensorData().stereoCameraModel().isValidForProjection()?1:0);
(int)fromSignature.sensorData().stereoCameraModels().size());
UDEBUG("Input(%d): to=%d words, %d 3D words, %d words descriptors, %d kpts, %d kpts3D, %d descriptors, image=%dx%d models=%d stereo=%d",
toSignature.id(),
@@ -323,7 +323,7 @@ Transform RegistrationVis::computeTransformationImpl(
toSignature.sensorData().imageRaw().cols,
toSignature.sensorData().imageRaw().rows,
(int)toSignature.sensorData().cameraModels().size(),
toSignature.sensorData().stereoCameraModel().isValidForProjection()?1:0);
(int)toSignature.sensorData().stereoCameraModels().size());
std::string msg;
info.projectedIDs.clear();
@@ -487,17 +487,24 @@ Transform RegistrationVis::computeTransformationImpl(
bool guessSet = !guess.isIdentity() && !guess.isNull();
if(guessSet)
{
Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModel().left().localTransform();
Transform guessCameraRef = (guess * localTransform).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModel().left().K();
cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
if(fromSignature.sensorData().cameraModels().size() == 1 || fromSignature.sensorData().cameraModels().size() == 1)
{
Transform localTransform = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].localTransform():fromSignature.sensorData().stereoCameraModels()[0].left().localTransform();
Transform guessCameraRef = (guess * localTransform).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
cv::Mat K = fromSignature.sensorData().cameraModels().size()?fromSignature.sensorData().cameraModels()[0].K():fromSignature.sensorData().stereoCameraModels()[0].left().K();
cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), cornersTo);
}
else
{
UERROR("Optical flow guess with multi-cameras is not implemented, guess ignored...");
}
}
// Find features in the new left image
@@ -735,7 +742,7 @@ Transform RegistrationVis::computeTransformationImpl(
if(!kptsFrom3D.empty() &&
(_detectorFrom->getMinDepth() > 0.0f || _detectorFrom->getMaxDepth() > 0.0f) &&
(!fromSignature.sensorData().cameraModels().empty() || fromSignature.sensorData().stereoCameraModel().isValidForProjection())) // Ignore local map from OdometryF2M
(!fromSignature.sensorData().cameraModels().empty() || !fromSignature.sensorData().stereoCameraModels().empty())) // Ignore local map from OdometryF2M
{
_detectorFrom->filterKeypointsByDepth(kptsFrom, descriptorsFrom, kptsFrom3D, _detectorFrom->getMinDepth(), _detectorFrom->getMaxDepth());
}
@@ -770,7 +777,7 @@ Transform RegistrationVis::computeTransformationImpl(
if(kptsTo3D.size() &&
(_detectorTo->getMinDepth() > 0.0f || _detectorTo->getMaxDepth() > 0.0f) &&
(!toSignature.sensorData().cameraModels().empty() || toSignature.sensorData().stereoCameraModel().isValidForProjection())) // Ignore local map from OdometryF2M
(!toSignature.sensorData().cameraModels().empty() || !toSignature.sensorData().stereoCameraModels().empty())) // Ignore local map from OdometryF2M
{
_detectorTo->filterKeypointsByDepth(kptsTo, descriptorsTo, kptsTo3D, _detectorTo->getMinDepth(), _detectorTo->getMaxDepth());
}
@@ -787,15 +794,37 @@ Transform RegistrationVis::computeTransformationImpl(
// We have all data we need here, so match!
if(descriptorsFrom.rows > 0 && descriptorsTo.rows > 0)
{
cv::Size imageSize = imageTo.size();
bool isCalibrated = false; // multiple cameras not supported.
if(imageSize.height == 0 || imageSize.width == 0)
std::vector<CameraModel> models;
if(!toSignature.sensorData().stereoCameraModels().empty())
{
imageSize = toSignature.sensorData().cameraModels().size() == 1?toSignature.sensorData().cameraModels()[0].imageSize():toSignature.sensorData().stereoCameraModel().left().imageSize();
for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
{
models.push_back(toSignature.sensorData().stereoCameraModels()[i].left());
}
}
else
{
models = toSignature.sensorData().cameraModels();
}
isCalibrated = imageSize.height != 0 && imageSize.width != 0 &&
(toSignature.sensorData().cameraModels().size()==1?toSignature.sensorData().cameraModels()[0].isValidForProjection():toSignature.sensorData().stereoCameraModel().isValidForProjection());
bool isCalibrated = !models.empty();
for(size_t i=0; i<models.size() && isCalibrated; ++i)
{
isCalibrated = models[i].isValidForProjection();
// For old database formats
if(isCalibrated && (models[i].imageWidth()==0 || models[i].imageHeight()==0))
{
if(!toSignature.sensorData().imageRaw().empty())
{
models[i].setImageSize(cv::Size(toSignature.sensorData().imageRaw().cols/models.size(), toSignature.sensorData().imageRaw().rows));
}
else
{
isCalibrated = false;
}
}
}
// If guess is set, limit the search of matches using optical flow window size
bool guessSet = !guess.isIdentity() && !guess.isNull();
@@ -803,52 +832,62 @@ Transform RegistrationVis::computeTransformationImpl(
isCalibrated && // needed for projection
_estimationType != 2) // To make sure we match all features for 2D->2D
{
// Use guess to project 3D "from" keypoints into "to" image
UDEBUG("");
UASSERT((int)kptsTo.size() == descriptorsTo.rows);
UASSERT((int)kptsFrom3D.size() == descriptorsFrom.rows);
// Use guess to project 3D "from" keypoints into "to" image
if(toSignature.sensorData().cameraModels().size() > 1)
std::vector<cv::Point2f> cornersProjected;
std::vector<int> projectedIndexToDescIndex;
float subImageWidth = models[0].imageWidth();
std::set<int> added;
int duplicates=0;
for(size_t m=0; m<models.size(); ++m)
{
UFATAL("Guess reprojection feature matching is not supported for multiple cameras.");
}
Transform guessCameraRef = (guess * models[m].localTransform()).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
cv::Mat K = models[m].K();
std::vector<cv::Point2f> projected;
cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
UDEBUG("Projected points=%d", (int)projected.size());
Transform localTransform = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].localTransform():toSignature.sensorData().stereoCameraModel().left().localTransform();
Transform guessCameraRef = (guess * localTransform).inverse();
cv::Mat R = (cv::Mat_<double>(3,3) <<
(double)guessCameraRef.r11(), (double)guessCameraRef.r12(), (double)guessCameraRef.r13(),
(double)guessCameraRef.r21(), (double)guessCameraRef.r22(), (double)guessCameraRef.r23(),
(double)guessCameraRef.r31(), (double)guessCameraRef.r32(), (double)guessCameraRef.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) << (double)guessCameraRef.x(), (double)guessCameraRef.y(), (double)guessCameraRef.z());
cv::Mat K = toSignature.sensorData().cameraModels().size()?toSignature.sensorData().cameraModels()[0].K():toSignature.sensorData().stereoCameraModel().left().K();
std::vector<cv::Point2f> projected;
cv::projectPoints(kptsFrom3D, rvec, tvec, K, cv::Mat(), projected);
UDEBUG("Projected points=%d", (int)projected.size());
//remove projected points outside of the image
UASSERT((int)projected.size() == descriptorsFrom.rows);
std::vector<cv::Point2f> cornersProjected(projected.size());
std::vector<int> projectedIndexToDescIndex(projected.size());
int oi=0;
for(unsigned int i=0; i<projected.size(); ++i)
{
if(uIsInBounds(projected[i].x, 0.0f, float(imageSize.width-1)) &&
uIsInBounds(projected[i].y, 0.0f, float(imageSize.height-1)) &&
util3d::transformPoint(kptsFrom3D[i], guessCameraRef).z > 0.0)
//remove projected points outside of the image
UASSERT((int)projected.size() == descriptorsFrom.rows);
int cornersInFrame = 0;
for(unsigned int i=0; i<projected.size(); ++i)
{
projectedIndexToDescIndex[oi] = i;
cornersProjected[oi++] = projected[i];
if(uIsInBounds(projected[i].x, 0.0f, float(models[m].imageWidth()-1)) &&
uIsInBounds(projected[i].y, 0.0f, float(models[m].imageHeight()-1)) &&
util3d::transformPoint(kptsFrom3D[i], guessCameraRef).z > 0.0)
{
if(added.find(i) != added.end())
{
++duplicates;
continue;
}
projectedIndexToDescIndex.push_back(i);
projected[i].x += subImageWidth*float(m); // Convert in multicam stitched image
cornersProjected.push_back(projected[i]);
++cornersInFrame;
added.insert(i);
}
}
UDEBUG("corners in frame=%d (camera index=%ld)", cornersInFrame, m);
}
projectedIndexToDescIndex.resize(oi);
cornersProjected.resize(oi);
UDEBUG("corners in frame=%d", (int)cornersProjected.size());
// For each projected feature guess of "from" in "to", find its matching feature in
// the radius around the projected guess.
// TODO: do cross-check?
UDEBUG("guessMatchToProjection=%d, cornersProjected=%d", _guessMatchToProjection?1:0, (int)cornersProjected.size());
UDEBUG("guessMatchToProjection=%d, cornersProjected=%d orignalWordsFromIds=%d (added=%ld, duplicates=%d)",
_guessMatchToProjection?1:0, (int)cornersProjected.size(), (int)orignalWordsFromIds.size(),
added.size(), duplicates);
if(cornersProjected.size())
{
if(_guessMatchToProjection)
@@ -1147,19 +1186,9 @@ Transform RegistrationVis::computeTransformationImpl(
{
if(guessSet && _guessWinSize > 0 && kptsFrom3D.size() && !isCalibrated)
{
if(fromSignature.sensorData().cameraModels().size() > 1 || toSignature.sensorData().cameraModels().size() > 1)
{
UWARN("Finding correspondences with the guess cannot "
"be done with multiple cameras, global matching is "
"done instead. Please set \"%s\" to 0 to avoid this warning.",
Parameters::kVisCorGuessWinSize().c_str());
}
else
{
UWARN("Calibration not found! Finding correspondences "
"with the guess cannot be done, global matching is "
"done instead.");
}
UWARN("Calibration not found! Finding correspondences "
"with the guess cannot be done, global matching is "
"done instead.");
}
UDEBUG("");
@@ -1194,16 +1223,16 @@ Transform RegistrationVis::computeTransformationImpl(
descriptorsTo.type() == CV_32F &&
descriptorsFrom.type() == CV_32F &&
descriptorsFrom.rows == (int)kptsFrom.size() &&
imageSize.width > 0 && imageSize.height > 0)
models.size() == 1)
{
UDEBUG("Python matching");
matches = _pyMatcher->match(descriptorsTo, descriptorsFrom, kptsTo, kptsFrom, imageSize);
matches = _pyMatcher->match(descriptorsTo, descriptorsFrom, kptsTo, kptsFrom, models[0].imageSize());
}
else
{
if(_nnType == 6 && _pyMatcher)
{
UDEBUG("Invalid inputs for Python matching (desc type=%d, only float descriptors supported), doing bruteforce matching instead.", descriptorsFrom.type());
UDEBUG("Invalid inputs for Python matching (desc type=%d, only float descriptors supported, multicam not supported), doing bruteforce matching instead.", descriptorsFrom.type());
}
#else
{
@@ -1215,11 +1244,11 @@ Transform RegistrationVis::computeTransformationImpl(
if(_nnType == 7)
{
imageSizeFrom = imageFrom.size();
if(imageSizeFrom.height == 0 || imageSizeFrom.width == 0)
if((imageSizeFrom.height == 0 || imageSizeFrom.width == 0) && (fromSignature.sensorData().cameraModels().size() || fromSignature.sensorData().stereoCameraModels().size()))
{
imageSizeFrom = fromSignature.sensorData().cameraModels().size() == 1?fromSignature.sensorData().cameraModels()[0].imageSize():fromSignature.sensorData().stereoCameraModel().left().imageSize();
imageSizeFrom = fromSignature.sensorData().cameraModels().size() == 1?fromSignature.sensorData().cameraModels()[0].imageSize():fromSignature.sensorData().stereoCameraModels()[0].left().imageSize();
}
if(imageSize.height > 0 && imageSize.width > 0 &&
if(!models.empty() && models[0].imageSize().height > 0 && models[0].imageSize().width > 0 &&
imageSizeFrom.height > 0 && imageSizeFrom.width > 0)
{
doCrossCheck = false;
@@ -1239,8 +1268,9 @@ Transform RegistrationVis::computeTransformationImpl(
#if defined(HAVE_OPENCV_XFEATURES2D) && (CV_MAJOR_VERSION > 3 || (CV_MAJOR_VERSION==3 && CV_MINOR_VERSION >=4 && CV_SUBMINOR_VERSION >= 1))
if(!doCrossCheck)
{
UASSERT(!models.empty());
std::vector<cv::DMatch> matchesGMS;
cv::xfeatures2d::matchGMS(imageSize, imageSizeFrom, kptsTo, kptsFrom, matches, matchesGMS, _gmsWithRotation, _gmsWithScale, _gmsThresholdFactor);
cv::xfeatures2d::matchGMS(models[0].imageSize(), imageSizeFrom, kptsTo, kptsFrom, matches, matchesGMS, _gmsWithRotation, _gmsWithScale, _gmsThresholdFactor);
matches = matchesGMS;
}
#endif
@@ -1385,7 +1415,8 @@ Transform RegistrationVis::computeTransformationImpl(
if(_estimationType == 2) // Epipolar Geometry
{
UDEBUG("");
if(!signatureB->sensorData().stereoCameraModel().isValidForProjection() &&
if((signatureB->sensorData().stereoCameraModels().size() != 1 ||
!signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
(signatureB->sensorData().cameraModels().size() != 1 ||
!signatureB->sensorData().cameraModels()[0].isValidForProjection()))
{
@@ -1394,8 +1425,8 @@ Transform RegistrationVis::computeTransformationImpl(
else if((int)signatureA->getWords().size() >= _minInliers &&
(int)signatureB->getWords().size() >= _minInliers)
{
UASSERT(signatureA->sensorData().stereoCameraModel().isValidForProjection() || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection()));
const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModel().isValidForProjection()?signatureA->sensorData().stereoCameraModel().left():signatureA->sensorData().cameraModels()[0];
UASSERT((signatureA->sensorData().stereoCameraModels().size() == 1 && signatureA->sensorData().stereoCameraModels()[0].isValidForProjection()) || (signatureA->sensorData().cameraModels().size() == 1 && signatureA->sensorData().cameraModels()[0].isValidForProjection()));
const CameraModel & cameraModel = signatureA->sensorData().stereoCameraModels().size()?signatureA->sensorData().stereoCameraModels()[0].left():signatureA->sensorData().cameraModels()[0];
// we only need the camera transform, send guess words3 for scale estimation
Transform cameraTransform;
@@ -1479,16 +1510,22 @@ Transform RegistrationVis::computeTransformationImpl(
else if(_estimationType == 1) // PnP
{
UDEBUG("");
if(!signatureB->sensorData().stereoCameraModel().isValidForProjection() &&
(signatureB->sensorData().cameraModels().size() != 1 ||
!signatureB->sensorData().cameraModels()[0].isValidForProjection()))
if((signatureB->sensorData().stereoCameraModels().empty() || !signatureB->sensorData().stereoCameraModels()[0].isValidForProjection()) &&
(signatureB->sensorData().cameraModels().empty() || !signatureB->sensorData().cameraModels()[0].isValidForProjection()))
{
UERROR("Calibrated camera required (multi-cameras not supported). Id=%d Models=%d StereoModel=%d weight=%d",
UERROR("Calibrated camera required. Id=%d Models=%d StereoModels=%d weight=%d",
signatureB->id(),
(int)signatureB->sensorData().cameraModels().size(),
signatureB->sensorData().stereoCameraModel().isValidForProjection()?1:0,
signatureB->sensorData().stereoCameraModels().size(),
signatureB->getWeight());
}
#ifndef RTABMAP_OPENGV
else if(signatureB->sensorData().cameraModels().size() > 1)
{
UERROR("Multi-camera 2D-3D PnP registration is only available if rtabmap is built "
"with OpenGV dependency. Use 3D-3D registration approach instead for multi-camera.");
}
#endif
else
{
UDEBUG("words from3D=%d to2D=%d", (int)signatureA->getWords3().size(), (int)signatureB->getWords().size());
@@ -1496,9 +1533,6 @@ Transform RegistrationVis::computeTransformationImpl(
if((int)signatureA->getWords3().size() >= _minInliers &&
(int)signatureB->getWords().size() >= _minInliers)
{
UASSERT(signatureB->sensorData().stereoCameraModel().isValidForProjection() || (signatureB->sensorData().cameraModels().size() == 1 && signatureB->sensorData().cameraModels()[0].isValidForProjection()));
const CameraModel & cameraModel = signatureB->sensorData().stereoCameraModel().isValidForProjection()?signatureB->sensorData().stereoCameraModel().left():signatureB->sensorData().cameraModels()[0];
std::vector<int> inliersV;
std::vector<int> matchesV;
std::map<int, int> uniqueWordsA = uMultimapToMapUnique(signatureA->getWords());
@@ -1518,22 +1552,63 @@ Transform RegistrationVis::computeTransformationImpl(
words3B.insert(std::make_pair(iter->first, signatureB->getWords3()[iter->second]));
}
}
transforms[dir] = util3d::estimateMotion3DTo2D(
words3A,
wordsB,
cameraModel,
_minInliers,
_iterations,
_PnPReprojError,
_PnPFlags,
_PnPRefineIterations,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
words3B,
&covariances[dir],
&matchesV,
&inliersV);
inliers[dir] = inliersV;
matches[dir] = matchesV;
std::vector<CameraModel> models;
if(signatureB->sensorData().stereoCameraModels().size())
{
for(size_t i=0; i<signatureB->sensorData().stereoCameraModels().size(); ++i)
{
models.push_back(signatureB->sensorData().stereoCameraModels()[i].left());
}
}
else
{
models = signatureB->sensorData().cameraModels();
}
if(models.size()>1)
{
// Multi-Camera
UASSERT(models[0].isValidForProjection());
transforms[dir] = util3d::estimateMotion3DTo2D(
words3A,
wordsB,
models,
_minInliers,
_iterations,
_PnPReprojError,
_PnPFlags,
_PnPRefineIterations,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
words3B,
&covariances[dir],
&matchesV,
&inliersV);
inliers[dir] = inliersV;
matches[dir] = matchesV;
}
else
{
UASSERT(models.size() == 1 && models[0].isValidForProjection());
transforms[dir] = util3d::estimateMotion3DTo2D(
words3A,
wordsB,
models[0],
_minInliers,
_iterations,
_PnPReprojError,
_PnPFlags,
_PnPRefineIterations,
dir==0?(!guess.isNull()?guess:Transform::getIdentity()):!transforms[0].isNull()?transforms[0].inverse():(!guess.isNull()?guess.inverse():Transform::getIdentity()),
words3B,
&covariances[dir],
&matchesV,
&inliersV);
inliers[dir] = inliersV;
matches[dir] = matchesV;
}
UDEBUG("inliers: %d/%d", (int)inliersV.size(), (int)matchesV.size());
if(transforms[dir].isNull())
{
@@ -1652,8 +1727,8 @@ Transform RegistrationVis::computeTransformationImpl(
allInliers.size() &&
fromSignature.getWords3().size() &&
toSignature.getWords().size() &&
fromSignature.sensorData().cameraModels().size() <= 1 &&
toSignature.sensorData().cameraModels().size() <= 1)
(fromSignature.sensorData().stereoCameraModels().size() >= 1 || fromSignature.sensorData().cameraModels().size() >= 1) &&
(toSignature.sensorData().stereoCameraModels().size() >= 1 || toSignature.sensorData().cameraModels().size() >= 1))
{
UDEBUG("Refine with bundle adjustment");
Optimizer * sba = Optimizer::create(_bundleAdjustment==3?Optimizer::kTypeCeres:_bundleAdjustment==2?Optimizer::kTypeCVSBA:Optimizer::kTypeG2O, _bundleParameters);
@@ -1693,60 +1768,61 @@ Transform RegistrationVis::computeTransformationImpl(
std::map<int, Transform> optimizedPoses;
UASSERT(toSignature.sensorData().stereoCameraModel().isValidForProjection() ||
(toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForProjection()));
UASSERT((toSignature.sensorData().stereoCameraModels().size() >= 1 && toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) ||
(toSignature.sensorData().cameraModels().size() >= 1 && toSignature.sensorData().cameraModels()[0].isValidForProjection()));
std::map<int, CameraModel> models;
std::map<int, std::vector<CameraModel> > models;
Transform invLocalTransformFrom;
CameraModel cameraModelFrom;
if(fromSignature.sensorData().stereoCameraModel().isValidForProjection())
std::vector<CameraModel> cameraModelsFrom;
if(fromSignature.sensorData().stereoCameraModels().size())
{
cameraModelFrom = fromSignature.sensorData().stereoCameraModel().left();
// Set Tx=-baseline*fx for Stereo BA
cameraModelFrom = CameraModel(cameraModelFrom.fx(),
cameraModelFrom.fy(),
cameraModelFrom.cx(),
cameraModelFrom.cy(),
cameraModelFrom.localTransform(),
-fromSignature.sensorData().stereoCameraModel().baseline()*cameraModelFrom.fy());
invLocalTransformFrom = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
for(size_t i=0; i<fromSignature.sensorData().stereoCameraModels().size(); ++i)
{
CameraModel cameraModel = fromSignature.sensorData().stereoCameraModels()[i].left();
// Set Tx=-baseline*fx for Stereo BA
cameraModel = CameraModel(cameraModel.fx(),
cameraModel.fy(),
cameraModel.cx(),
cameraModel.cy(),
cameraModel.localTransform(),
-fromSignature.sensorData().stereoCameraModels()[0].baseline()*cameraModel.fx(),
cameraModel.imageSize());
cameraModelsFrom.push_back(cameraModel);
}
}
else if(fromSignature.sensorData().cameraModels().size() == 1)
else
{
cameraModelFrom = fromSignature.sensorData().cameraModels()[0];
invLocalTransformFrom = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
cameraModelsFrom = fromSignature.sensorData().cameraModels();
}
Transform invLocalTransformTo = Transform::getIdentity();
CameraModel cameraModelTo;
if(toSignature.sensorData().stereoCameraModel().isValidForProjection())
std::vector<CameraModel> cameraModelsTo;
if(toSignature.sensorData().stereoCameraModels().size())
{
cameraModelTo = toSignature.sensorData().stereoCameraModel().left();
// Set Tx=-baseline*fx for Stereo BA
cameraModelTo = CameraModel(cameraModelTo.fx(),
cameraModelTo.fy(),
cameraModelTo.cx(),
cameraModelTo.cy(),
cameraModelTo.localTransform(),
-toSignature.sensorData().stereoCameraModel().baseline()*cameraModelTo.fy());
invLocalTransformTo = toSignature.sensorData().stereoCameraModel().localTransform().inverse();
for(size_t i=0; i<toSignature.sensorData().stereoCameraModels().size(); ++i)
{
CameraModel cameraModel = toSignature.sensorData().stereoCameraModels()[i].left();
// Set Tx=-baseline*fx for Stereo BA
cameraModel = CameraModel(cameraModel.fx(),
cameraModel.fy(),
cameraModel.cx(),
cameraModel.cy(),
cameraModel.localTransform(),
-toSignature.sensorData().stereoCameraModels()[0].baseline()*cameraModel.fx(),
cameraModel.imageSize());
cameraModelsTo.push_back(cameraModel);
}
}
else if(toSignature.sensorData().cameraModels().size() == 1)
else
{
cameraModelTo = toSignature.sensorData().cameraModels()[0];
invLocalTransformTo = toSignature.sensorData().cameraModels()[0].localTransform().inverse();
}
if(invLocalTransformFrom.isNull())
{
invLocalTransformFrom = invLocalTransformTo;
cameraModelsTo = toSignature.sensorData().cameraModels();
}
models.insert(std::make_pair(1, cameraModelFrom.isValidForProjection()?cameraModelFrom:cameraModelTo));
models.insert(std::make_pair(2, cameraModelTo));
models.insert(std::make_pair(1, cameraModelsFrom));
models.insert(std::make_pair(2, cameraModelsTo));
std::map<int, std::map<int, FeatureBA> > wordReferences;
std::set<int> sbaOutliers;
UDEBUG("");
for(unsigned int i=0; i<allInliers.size(); ++i)
{
int wordId = allInliers[i];
@@ -1762,22 +1838,50 @@ Transform RegistrationVis::computeTransformationImpl(
points3DMap.insert(std::make_pair(wordId, pt3D));
std::map<int, FeatureBA> ptMap;
if(!fromSignature.getWordsKpts().empty() && cameraModelFrom.isValidForProjection())
if(!fromSignature.getWordsKpts().empty())
{
float depthFrom = util3d::transformPoint(pt3D, invLocalTransformFrom).z;
const cv::KeyPoint & kpt = fromSignature.getWordsKpts()[indexFrom];
ptMap.insert(std::make_pair(1,FeatureBA(kpt, depthFrom)));
cv::KeyPoint kpt = fromSignature.getWordsKpts()[indexFrom];
int cameraIndex = 0;
const std::vector<CameraModel> & cam = models.at(1);
if(cam.size()>1)
{
UASSERT(cam[0].imageWidth()>0);
float subImageWidth = cam[0].imageWidth();
cameraIndex = int(kpt.pt.x / subImageWidth);
kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
}
UASSERT(cam[cameraIndex].isValidForProjection());
float depthFrom = util3d::transformPoint(pt3D, cam[cameraIndex].localTransform().inverse()).z;
ptMap.insert(std::make_pair(1,FeatureBA(kpt, depthFrom, cv::Mat(), cameraIndex)));
}
if(!toSignature.getWordsKpts().empty() && cameraModelTo.isValidForProjection())
if(!toSignature.getWordsKpts().empty())
{
int indexTo = toSignature.getWords().find(wordId)->second;
cv::KeyPoint kpt = toSignature.getWordsKpts()[indexTo];
int cameraIndex = 0;
const std::vector<CameraModel> & cam = models.at(2);
if(cam.size()>1)
{
UASSERT(cam[0].imageWidth()>0);
float subImageWidth = cam[0].imageWidth();
cameraIndex = int(kpt.pt.x / subImageWidth);
kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
}
UASSERT(cam[cameraIndex].isValidForProjection());
float depthTo = 0.0f;
if(!toSignature.getWords3().empty())
{
depthTo = util3d::transformPoint(toSignature.getWords3()[indexTo], invLocalTransformTo).z;
depthTo = util3d::transformPoint(toSignature.getWords3()[indexTo], cam[cameraIndex].localTransform().inverse()).z;
}
const cv::KeyPoint & kpt = toSignature.getWordsKpts()[indexTo];
ptMap.insert(std::make_pair(2,FeatureBA(kpt, depthTo)));
ptMap.insert(std::make_pair(2,FeatureBA(kpt, depthTo, cv::Mat(), cameraIndex)));
}
wordReferences.insert(std::make_pair(wordId, ptMap));
@@ -1876,10 +1980,10 @@ Transform RegistrationVis::computeTransformationImpl(
float cx=0, cy=0, w=0, h=0;
if(_minInliersDistributionThr > 0)
{
if(toSignature.sensorData().stereoCameraModel().isValidForProjection() ||
if((toSignature.sensorData().stereoCameraModels().size() == 1 && toSignature.sensorData().stereoCameraModels()[0].isValidForProjection()) ||
(toSignature.sensorData().cameraModels().size() == 1 && toSignature.sensorData().cameraModels()[0].isValidForReprojection()))
{
const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModel().isValidForProjection()?toSignature.sensorData().stereoCameraModel().left():toSignature.sensorData().cameraModels()[0];
const CameraModel & cameraModel = toSignature.sensorData().stereoCameraModels().size()?toSignature.sensorData().stereoCameraModels()[0].left():toSignature.sensorData().cameraModels()[0];
cx = cameraModel.cx();
cy = cameraModel.cy();
w = cameraModel.imageWidth();
@@ -1894,7 +1998,7 @@ Transform RegistrationVis::computeTransformationImpl(
UERROR("Invalid calibration image size (%dx%d), cannot compute inliers distribution! (see %s=%f)", w, h, Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
}
}
else if(toSignature.sensorData().cameraModels().size() > 1)
else if(toSignature.sensorData().cameraModels().size() > 1 || toSignature.sensorData().stereoCameraModels().size() > 1)
{
UERROR("Multi-camera not supported when computing inliers distribution! (see %s=%f)", Parameters::kVisMinInliersDistribution().c_str(), _minInliersDistributionThr);
}

6
corelib/src/Rtabmap.cpp
View File

@@ -5007,10 +5007,10 @@ Signature Rtabmap::getSignatureCopy(int id, bool images, bool scan, bool userDat
if(!images && withWords)
{
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
_memory->getNodeCalibration(id, models, stereoModel);
std::vector<StereoCameraModel> stereoModels;
_memory->getNodeCalibration(id, models, stereoModels);
data.setCameraModels(models);
data.setStereoCameraModel(stereoModel);
data.setStereoCameraModels(stereoModels);
}
s=Signature(id,

75
corelib/src/SensorData.cpp
View File

@@ -175,6 +175,40 @@ SensorData::SensorData(
setUserData(userData);
}
// Multi-Stereo constructor
SensorData::SensorData(
const cv::Mat & left,
const cv::Mat & right,
const std::vector<StereoCameraModel> & cameraModels,
int id,
double stamp,
const cv::Mat & userData):
_id(id),
_stamp(stamp),
_cellSize(0.0f)
{
setStereoImage(left, right, cameraModels);
setUserData(userData);
}
// Multi-Stereo constructor + 2d laser scan
SensorData::SensorData(
const LaserScan & laserScan,
const cv::Mat & left,
const cv::Mat & right,
const std::vector<StereoCameraModel> & cameraModels,
int id,
double stamp,
const cv::Mat & userData) :
_id(id),
_stamp(stamp),
_cellSize(0.0f)
{
setStereoImage(left, right, cameraModels);
setLaserScan(laserScan);
setUserData(userData);
}
SensorData::SensorData(
const IMU & imu,
int id,
@@ -206,16 +240,16 @@ void SensorData::setRGBDImage(
const std::vector<CameraModel> & models,
bool clearPreviousData)
{
if(!clearPreviousData && _stereoCameraModel.isValidForProjection())
if(!clearPreviousData && !_stereoCameraModels.empty())
{
UERROR("Sensor data has previously stereo images "
"but clearPreviousData parameter is false. We "
"will still clear previous data to avoid incompatibilities "
"between raw and compressed data!");
}
bool clearData = clearPreviousData || _stereoCameraModel.isValidForProjection();
bool clearData = clearPreviousData || !_stereoCameraModels.empty();
_stereoCameraModel = StereoCameraModel();
_stereoCameraModels.clear();
_cameraModels = models;
if(rgb.rows == 1)
{
@@ -272,6 +306,16 @@ void SensorData::setStereoImage(
const cv::Mat & right,
const StereoCameraModel & stereoCameraModel,
bool clearPreviousData)
{
std::vector<StereoCameraModel> models;
models.push_back(stereoCameraModel);
setStereoImage(left, right, models, clearPreviousData);
}
void SensorData::setStereoImage(
const cv::Mat & left,
const cv::Mat & right,
const std::vector<StereoCameraModel> & stereoCameraModels,
bool clearPreviousData)
{
if(!clearPreviousData && !_cameraModels.empty())
{
@@ -283,7 +327,7 @@ void SensorData::setStereoImage(
bool clearData = clearPreviousData || !_cameraModels.empty();
_cameraModels.clear();
_stereoCameraModel = stereoCameraModel;
_stereoCameraModels = stereoCameraModels;
if(left.rows == 1)
{
@@ -842,15 +886,24 @@ bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
}
}
}
else if(_stereoCameraModel.isValidForProjection())
else if(_stereoCameraModels.size() >= 1)
{
cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _stereoCameraModel.localTransform().inverse());
if(ptInCameraFrame.z > 0.0f)
for(unsigned int i=0; i<_stereoCameraModels.size(); ++i)
{
int u, v;
_stereoCameraModel.left().reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
return uIsInBounds(u, 0, _stereoCameraModel.left().imageWidth()) &&
uIsInBounds(v, 0, _stereoCameraModel.left().imageHeight());
if(_stereoCameraModels[i].isValidForProjection() && !_stereoCameraModels[i].localTransform().isNull())
{
cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _stereoCameraModels[i].localTransform().inverse());
if(ptInCameraFrame.z > 0.0f)
{
int u, v;
_stereoCameraModels[i].left().reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
if(uIsInBounds(u, 0, _stereoCameraModels[i].left().imageWidth()) &&
uIsInBounds(v, 0, _stereoCameraModels[i].left().imageHeight()))
{
return true;
}
}
}
}
}
else

12
corelib/src/Stereo.cpp
View File

@@ -148,18 +148,6 @@ std::vector<cv::Point2f> StereoOpticalFlow::computeCorrespondences(
}
UDEBUG("total=%d countFlowRejected=%d countDisparityRejected=%d", (int)status.size(), countFlowRejected, countDisparityRejected);
if(countFlowRejected + countDisparityRejected > (int)status.size()/2)
{
UWARN("A large number (%d/%d) of stereo correspondences are rejected! "
"Optical flow may have failed because images are not calibrated, "
"the background is too far (no disparity between the images), "
"maximum disparity may be too small (%f) or that exposure between "
"left and right images is too different.",
countFlowRejected+countDisparityRejected,
(int)status.size(),
this->maxDisparity());
}
return rightCorners;
}

13
corelib/src/Transform.cpp
View File

@@ -442,6 +442,19 @@ Transform Transform::fromEigen3d(const Eigen::Isometry3d & matrix)
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
Transform Transform::fromEigen3f(const Eigen::Matrix<float, 3, 4> & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
Transform Transform::fromEigen3d(const Eigen::Matrix<double, 3, 4> & matrix)
{
return Transform(matrix(0,0), matrix(0,1), matrix(0,2), matrix(0,3),
matrix(1,0), matrix(1,1), matrix(1,2), matrix(1,3),
matrix(2,0), matrix(2,1), matrix(2,2), matrix(2,3));
}
/**
* Format (3 values): x y z
* Format (6 values): x y z roll pitch yaw

14
corelib/src/camera/CameraDepthAI.cpp
View File

@@ -55,7 +55,8 @@ CameraDepthAI::CameraDepthAI(
deviceSerial_(deviceSerial),
outputDepth_(false),
depthConfidence_(200),
resolution_(resolution)
resolution_(resolution),
imuFirmwareUpdate_(false)
#endif
{
#ifdef RTABMAP_DEPTHAI
@@ -86,6 +87,15 @@ void CameraDepthAI::setOutputDepth(bool enabled, int confidence)
#endif
}
void CameraDepthAI::setIMUFirmwareUpdate(bool enabled)
{
#ifdef RTABMAP_DEPTHAI
imuFirmwareUpdate_ = enabled;
#else
UERROR("CameraDepthAI: RTAB-Map is not built with depthai-core support!");
#endif
}
bool CameraDepthAI::init(const std::string & calibrationFolder, const std::string & cameraName)
{
UDEBUG("");
@@ -195,6 +205,8 @@ bool CameraDepthAI::init(const std::string & calibrationFolder, const std::strin
// Link plugins IMU -> XLINK
imu->out.link(xoutIMU->input);
imu->enableFirmwareUpdate(imuFirmwareUpdate_);
device_.reset(new dai::Device(p, deviceToUse));
UINFO("Loading eeprom calibration data");

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

@@ -73,9 +73,10 @@ Transform OdometryF2F::computeTransform(
{
UTimer timer;
Transform output;
if(!data.rightRaw().empty() && !data.stereoCameraModel().isValidForProjection())
if(!data.rightRaw().empty() &&
(data.stereoCameraModels().size() != 1 || !data.stereoCameraModels()[0].isValidForProjection()))
{
UERROR("Calibrated stereo camera required");
UERROR("Calibrated stereo camera required (multi-cameras not supported)");
return output;
}
if(!data.depthRaw().empty() &&

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

@@ -225,12 +225,6 @@ Transform OdometryF2M::computeTransform(
lastFrame_ = new Signature(data);
data.setId(id);
if(bundleAdjustment_ > 0 &&
data.cameraModels().size() > 1)
{
UERROR("Odometry bundle adjustment doesn't work with multi-cameras. It is disabled.");
bundleAdjustment_ = 0;
}
bool addKeyFrame = false;
int totalBundleWordReferencesUsed = 0;
int totalBundleOutliers = 0;
@@ -252,7 +246,7 @@ Transform OdometryF2M::computeTransform(
std::map<int, cv::Point3f> points3DMap;
std::map<int, Transform> bundlePoses;
std::multimap<int, Link> bundleLinks;
std::map<int, CameraModel> bundleModels;
std::map<int, std::vector<CameraModel> > bundleModels;
for(int guessIteration=0;
guessIteration<(!guess.isNull()&&regPipeline_->isImageRequired()?2:1) && transform.isNull();
@@ -315,7 +309,7 @@ Transform OdometryF2M::computeTransform(
// local bundle adjustment
if(bundleAdjustment_>0 && sba_ &&
regPipeline_->isImageRequired() &&
lastFrame_->sensorData().cameraModels().size() <= 1 && // multi-cameras not supported
(!lastFrame_->sensorData().stereoCameraModels().empty() || !lastFrame_->sensorData().cameraModels().empty()) &&
regInfo.inliersIDs.size())
{
UDEBUG("Local Bundle Adjustment");
@@ -326,7 +320,12 @@ Transform OdometryF2M::computeTransform(
map_->getWords().begin()->first != tmpMap.getWords().begin()->first ||
map_->getWords().rbegin()->first != tmpMap.getWords().rbegin()->first)
{
UERROR("Bundle Adjustment cannot be used with a registration approach recomputing features from the \"from\" signature (e.g., Optical Flow).");
UERROR("Bundle Adjustment cannot be used with a registration approach recomputing "
"features from the \"from\" signature (e.g., Optical Flow) that would change "
"their ids (size=old=%ld new=%ld first/last: old=%d->%d new=%d->%d).",
map_->getWords().size(), tmpMap.getWords().size(),
map_->getWords().begin()->first, map_->getWords().rbegin()->first,
tmpMap.getWords().begin()->first, tmpMap.getWords().rbegin()->first);
bundleAdjustment_ = 0;
}
else
@@ -350,28 +349,34 @@ Transform OdometryF2M::computeTransform(
bundleLinks.insert(std::make_pair(lastFrame_->id(), Link(lastFrame_->id(), lastFrame_->id(), Link::kGravity, imuT)));
}
CameraModel model;
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
std::vector<CameraModel> models;
if(!lastFrame_->sensorData().cameraModels().empty() &&
lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
model = lastFrame_->sensorData().cameraModels()[0];
models = lastFrame_->sensorData().cameraModels();
}
else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
else if(!lastFrame_->sensorData().stereoCameraModels().empty() &&
lastFrame_->sensorData().stereoCameraModels().at(0).isValidForProjection())
{
model = lastFrame_->sensorData().stereoCameraModel().left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
for(size_t i=0; i<lastFrame_->sensorData().stereoCameraModels().size(); ++i)
{
CameraModel model = lastFrame_->sensorData().stereoCameraModels()[i].left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-lastFrame_->sensorData().stereoCameraModels()[i].baseline()*model.fx(),
model.imageSize());
models.push_back(model);
}
}
else
{
UFATAL("no valid camera model to do odometry bundle adjustment!");
}
bundleModels.insert(std::make_pair(lastFrame_->id(), model));
Transform invLocalTransform = model.localTransform().inverse();
bundleModels.insert(std::make_pair(lastFrame_->id(), models));
UDEBUG("Fill matches (%d)", (int)regInfo.inliersIDs.size());
std::map<int, std::map<int, FeatureBA> > wordReferences;
@@ -416,15 +421,27 @@ Transform OdometryF2M::computeTransform(
if(iter2D!=lastFrame_->getWords().end())
{
UASSERT(!lastFrame_->getWordsKpts().empty());
cv::KeyPoint kpt = lastFrame_->getWordsKpts()[iter2D->second];
int cameraIndex = 0;
const std::vector<CameraModel> & cam = bundleModels.at(lastFrame_->id());
if(cam.size()>1)
{
UASSERT(cam[0].imageWidth()>0);
float subImageWidth = cam[0].imageWidth();
cameraIndex = int(kpt.pt.x / subImageWidth);
kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
}
//get depth
float d = 0.0f;
if( !lastFrame_->getWords3().empty() &&
util3d::isFinite(lastFrame_->getWords3()[iter2D->second]))
{
//move back point in camera frame (to get depth along z)
d = util3d::transformPoint(lastFrame_->getWords3()[iter2D->second], invLocalTransform).z;
d = util3d::transformPoint(lastFrame_->getWords3()[iter2D->second], cam[cameraIndex].localTransform().inverse()).z;
}
references.insert(std::make_pair(lastFrame_->id(), FeatureBA(lastFrame_->getWordsKpts()[iter2D->second], d)));
references.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, d, cv::Mat(), cameraIndex)));
}
wordReferences.insert(std::make_pair(wordId, references));
@@ -626,26 +643,10 @@ Transform OdometryF2M::computeTransform(
}
// sort by feature response
std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > > newIds;
std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, std::pair<cv::Mat, int> > > > > newIds;
UASSERT(lastFrame_->getWords3().size() == lastFrame_->getWords().size());
UDEBUG("new frame words3=%d", (int)lastFrame_->getWords3().size());
std::set<int> seenStatusUpdated;
Transform invLocalTransform;
if(bundleAdjustment_>0)
{
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
invLocalTransform = lastFrame_->sensorData().cameraModels()[0].localTransform().inverse();
}
else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
{
invLocalTransform = lastFrame_->sensorData().stereoCameraModel().left().localTransform().inverse();
}
else
{
UFATAL("no valid camera model!");
}
}
// add points without depth only if the local map has reached its maximum size
bool addPointsWithoutDepth = false;
@@ -673,7 +674,18 @@ Transform OdometryF2M::computeTransform(
for(std::multimap<int, int>::const_iterator iter = lastFrame_->getWords().begin(); iter!=lastFrame_->getWords().end(); ++iter)
{
const cv::Point3f & pt = lastFrame_->getWords3()[iter->second];
const cv::KeyPoint & kpt = lastFrame_->getWordsKpts()[iter->second];
cv::KeyPoint kpt = lastFrame_->getWordsKpts()[iter->second];
int cameraIndex = 0;
const std::vector<CameraModel> & cam = bundleModels.at(lastFrame_->id());
if(cam.size()>1)
{
UASSERT(cam[0].imageWidth()>0);
float subImageWidth = cam[0].imageWidth();
cameraIndex = int(kpt.pt.x / subImageWidth);
kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
}
if(mapWords.find(iter->first) == mapWords.end()) // Point not in map
{
if(util3d::isFinite(pt) || addPointsWithoutDepth)
@@ -682,7 +694,8 @@ Transform OdometryF2M::computeTransform(
std::make_pair(kpt.response>0?1.0f/kpt.response:0.0f,
std::make_pair(iter->first,
std::make_pair(kpt,
std::make_pair(pt, lastFrame_->getWordsDescriptors().row(iter->second))))));
std::make_pair(pt,
std::make_pair(lastFrame_->getWordsDescriptors().row(iter->second), cameraIndex))))));
}
}
else if(bundleAdjustment_>0)
@@ -702,17 +715,17 @@ Transform OdometryF2M::computeTransform(
float depth = 0.0f;
if(util3d::isFinite(pt))
{
depth = util3d::transformPoint(pt, invLocalTransform).z;
depth = util3d::transformPoint(pt, cam[cameraIndex].localTransform().inverse()).z;
}
if(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end())
{
std::map<int, FeatureBA> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth)));
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex)));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
else
{
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth)));
bundleWordReferences_.find(iter->first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, depth, cv::Mat(), cameraIndex)));
}
}
}
@@ -721,7 +734,9 @@ Transform OdometryF2M::computeTransform(
int lastFrameOldestNewId = lastFrameOldestNewId_;
lastFrameOldestNewId_ = lastFrame_->getWords().size()?lastFrame_->getWords().rbegin()->first:0;
for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, cv::Mat> > > >::reverse_iterator iter=newIds.rbegin();
const std::vector<CameraModel> * cam = bundleModels.find(lastFrame_->id()) != bundleModels.end()?&bundleModels.at(lastFrame_->id()):0;
UASSERT(bundleAdjustment_ == 0 || cam);
for(std::multimap<float, std::pair<int, std::pair<cv::KeyPoint, std::pair<cv::Point3f, std::pair<cv::Mat, int> > > > >::reverse_iterator iter=newIds.rbegin();
iter!=newIds.rend();
++iter)
{
@@ -736,19 +751,20 @@ Transform OdometryF2M::computeTransform(
//move back point in camera frame (to get depth along z)
float depth = 0.0f;
int cameraIndex = iter->second.second.second.second.second;
if(util3d::isFinite(iter->second.second.second.first))
{
depth = util3d::transformPoint(iter->second.second.second.first, invLocalTransform).z;
depth = util3d::transformPoint(iter->second.second.second.first, (*cam)[cameraIndex].localTransform().inverse()).z;
}
if(bundleWordReferences_.find(iter->second.first) == bundleWordReferences_.end())
{
std::map<int, FeatureBA> framePt;
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth)));
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth, cv::Mat(), cameraIndex)));
bundleWordReferences_.insert(std::make_pair(iter->second.first, framePt));
}
else
{
bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth)));
bundleWordReferences_.find(iter->second.first)->second.insert(std::make_pair(lastFrame_->id(), FeatureBA(iter->second.second.first, depth, cv::Mat(), cameraIndex)));
}
}
}
@@ -774,9 +790,16 @@ Transform OdometryF2M::computeTransform(
{
model = lastFrame_->sensorData().cameraModels()[0];
}
else
else if(lastFrame_->sensorData().stereoCameraModels().size() > 1)
{
model = lastFrame_->sensorData().stereoCameraModel().left();
subImageWidth = lastFrame_->sensorData().imageRaw().cols/lastFrame_->sensorData().stereoCameraModels().size();
int cameraIndex = int(x / subImageWidth);
model = lastFrame_->sensorData().stereoCameraModels()[cameraIndex].left();
x = x-subImageWidth*cameraIndex;
}
else if(lastFrame_->sensorData().stereoCameraModels().size() == 1)
{
model = lastFrame_->sensorData().stereoCameraModels()[0].left();
}
Eigen::Vector3f ray = util3d::projectDepthTo3DRay(
@@ -791,7 +814,7 @@ Transform OdometryF2M::computeTransform(
pt = util3d::transformPoint(cv::Point3f(ray[0]*scaleInf, ray[1]*scaleInf, ray[2]*scaleInf), model.localTransform()); // in base_link frame
}
mapPoints.push_back(util3d::transformPoint(pt, newFramePose));
mapDescriptors.push_back(iter->second.second.second.second);
mapDescriptors.push_back(iter->second.second.second.second.first);
if(lastFrameOldestNewId_ > iter->second.first)
{
lastFrameOldestNewId_ = iter->second.first;
@@ -799,6 +822,7 @@ Transform OdometryF2M::computeTransform(
++added;
}
}
UDEBUG("");
// remove words in map if max size is reached
if((int)mapWords.size() > maximumMapSize_)
@@ -1207,18 +1231,32 @@ Transform OdometryF2M::computeTransform(
if(bundleAdjustment_>0)
{
Transform invLocalTransform;
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
std::vector<CameraModel> models;
if(!lastFrame_->sensorData().cameraModels().empty() &&
lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
invLocalTransform = lastFrame_->sensorData().cameraModels()[0].localTransform().inverse();
models = lastFrame_->sensorData().cameraModels();
}
else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
else if(!lastFrame_->sensorData().stereoCameraModels().empty() &&
lastFrame_->sensorData().stereoCameraModels().at(0).isValidForProjection())
{
invLocalTransform = lastFrame_->sensorData().stereoCameraModel().left().localTransform().inverse();
for(size_t i=0; i<lastFrame_->sensorData().stereoCameraModels().size(); ++i)
{
CameraModel model = lastFrame_->sensorData().stereoCameraModels()[i].left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-lastFrame_->sensorData().stereoCameraModels()[i].baseline()*model.fx(),
model.imageSize());
models.push_back(model);
}
}
else
{
UFATAL("no valid camera model!");
UFATAL("invalid camera model!");
}
// update bundleWordReferences_: used for bundle adjustment
@@ -1231,6 +1269,17 @@ Transform OdometryF2M::computeTransform(
UASSERT(bundleWordReferences_.find(iter->first) == bundleWordReferences_.end());
std::map<int, FeatureBA> framePt;
cv::KeyPoint kpt = wordsKpts[iter->second];
int cameraIndex = 0;
if(models.size()>1)
{
UASSERT(models[0].imageWidth()>0);
float subImageWidth = models[0].imageWidth();
cameraIndex = int(kpt.pt.x / subImageWidth);
kpt.pt.x = kpt.pt.x - (subImageWidth*float(cameraIndex));
}
//get depth
float d = 0.0f;
if(lastFrame_->getWords().count(iter->first) == 1 &&
@@ -1238,39 +1287,18 @@ Transform OdometryF2M::computeTransform(
util3d::isFinite(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second]))
{
//move back point in camera frame (to get depth along z)
d = util3d::transformPoint(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second], invLocalTransform).z;
d = util3d::transformPoint(lastFrame_->getWords3()[lastFrame_->getWords().find(iter->first)->second], models[cameraIndex].localTransform().inverse()).z;
}
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(wordsKpts[iter->second], d)));
framePt.insert(std::make_pair(lastFrame_->id(), FeatureBA(kpt, d, cv::Mat(), cameraIndex)));
bundleWordReferences_.insert(std::make_pair(iter->first, framePt));
}
}
}
bundlePoseReferences_.insert(std::make_pair(lastFrame_->id(), (int)bundleWordReferences_.size()));
CameraModel model;
if(lastFrame_->sensorData().cameraModels().size() == 1 && lastFrame_->sensorData().cameraModels().at(0).isValidForProjection())
{
model = lastFrame_->sensorData().cameraModels()[0];
}
else if(lastFrame_->sensorData().stereoCameraModel().isValidForProjection())
{
model = lastFrame_->sensorData().stereoCameraModel().left();
// Set Tx for stereo BA
model = CameraModel(model.fx(),
model.fy(),
model.cx(),
model.cy(),
model.localTransform(),
-lastFrame_->sensorData().stereoCameraModel().baseline()*model.fx());
}
else
{
UFATAL("invalid camera model!");
}
bundleModels_.insert(std::make_pair(lastFrame_->id(), model));
bundleModels_.insert(std::make_pair(lastFrame_->id(), models));
bundlePoses_.insert(std::make_pair(lastFrame_->id(), newFramePose));
if(!imuT.isNull())

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

@@ -170,7 +170,8 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
return output;
}
if(!(((data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection()) || data.stereoCameraModel().isValidForProjection())))
if(!((data.cameraModels().size() == 1 && data.cameraModels()[0].isValidForProjection()) ||
(data.stereoCameraModels().size() == 1 && data.stereoCameraModels()[0].isValidForProjection())))
{
UERROR("Odometry cannot be done without calibration or on multi-camera!");
return output;
@@ -178,21 +179,24 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
CameraModel cameraModel;
if(data.stereoCameraModel().isValidForProjection())
if(data.stereoCameraModels().size())
{
cameraModel = data.stereoCameraModel().left();
cameraModel = data.stereoCameraModels()[0].left();
// Set Tx for stereo BA
cameraModel = CameraModel(cameraModel.fx(),
cameraModel.fy(),
cameraModel.cx(),
cameraModel.cy(),
cameraModel.localTransform(),
-data.stereoCameraModel().baseline()*cameraModel.fx());
-data.stereoCameraModels()[0].baseline()*cameraModel.fx(),
cameraModel.imageSize());
}
else
{
cameraModel = data.cameraModels()[0];
}
std::vector<CameraModel> newModel;
newModel.push_back(cameraModel);
UTimer timer;
@@ -205,9 +209,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
{
cv::Mat newFrame;
cv::cvtColor(data.imageRaw(), newFrame, cv::COLOR_BGR2GRAY);
if(data.stereoCameraModel().isValidForProjection())
if(!data.stereoCameraModels().empty())
{
data.setStereoImage(newFrame, data.rightRaw(), data.stereoCameraModel());
data.setStereoImage(newFrame, data.rightRaw(), data.stereoCameraModels());
}
else
{
@@ -436,9 +440,9 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
{
UWARN("Bundle adjustment: fill arguments");
std::multimap<int, Link> links = keyFrameLinks_;
std::map<int, CameraModel> models = keyFrameModels_;
std::map<int, std::vector<CameraModel> > models = keyFrameModels_;
links.insert(std::make_pair(keyFramePoses_.rbegin()->first, newLink));
models.insert(std::make_pair(newS->id(), cameraModel));
models.insert(std::make_pair(newS->id(), newModel));
std::map<int, std::map<int, FeatureBA> > wordReferences;
for(std::set<int>::iterator iter = memory_->getStMem().begin(); iter!=memory_->getStMem().end(); ++iter)
@@ -596,7 +600,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
}
keyFramePoses_ = poses;
keyFrameLinks_.insert(std::make_pair(newLink.from(), newLink));
keyFrameModels_.insert(std::make_pair(newS->id(), cameraModel));
keyFrameModels_.insert(std::make_pair(newS->id(), newModel));
// keep only the two last signatures
while(localHistoryMaxSize_ && (int)localMap_.size() > localHistoryMaxSize_ && memory_->getStMem().size()>2)
@@ -798,7 +802,7 @@ Transform OdometryMono::computeTransform(SensorData & data, const Transform & gu
keyFrameWords3D_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), refWords3));
}
keyFramePoses_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), this->getPose()));
keyFrameModels_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), cameraModel));
keyFrameModels_.insert(std::make_pair(memory_->getLastWorkingSignature()->id(), newModel));
}
}
else

5
corelib/src/opencv/solvepnp.cpp
View File

@@ -137,9 +137,6 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
CV_Assert(ipoints.depth() == CV_32F || ipoints.depth() == CV_64F);
CV_Assert((ipoints.rows == 1 && ipoints.channels() == 2) || ipoints.cols*ipoints.channels() == 2);
_rvec.create(3, 1, CV_64FC1);
_tvec.create(3, 1, CV_64FC1);
Mat rvec = useExtrinsicGuess ? _rvec.getMat() : Mat(3, 1, CV_64FC1);
Mat tvec = useExtrinsicGuess ? _tvec.getMat() : Mat(3, 1, CV_64FC1);
Mat cameraMatrix = _cameraMatrix.getMat(), distCoeffs = _distCoeffs.getMat();
@@ -181,7 +178,7 @@ bool solvePnPRansac(InputArray _opoints, InputArray _ipoints,
opoints_inliers.resize(npoints1);
ipoints_inliers.resize(npoints1);
result = solvePnP(opoints_inliers, ipoints_inliers, cameraMatrix,
distCoeffs, rvec, tvec, false, flags == CV_P3P ? CV_EPNP : flags) ? 1 : -1;
distCoeffs, rvec, tvec, useExtrinsicGuess, flags == CV_P3P ? CV_EPNP : flags) ? 1 : -1;
}
if( result <= 0 || _local_model.rows <= 0)

2
corelib/src/optimizer/OptimizerCVSBA.cpp
View File

@@ -57,7 +57,7 @@ std::map<int, Transform> OptimizerCVSBA::optimizeBA(
int rootId,
const std::map<int, Transform> & posesIn,
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models,
const std::map<int, std::vector<CameraModel> > & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences, // <ID words, IDs frames + keypoint/Disparity>)
std::set<int> * outliers)

201
corelib/src/optimizer/OptimizerG2O.cpp
View File

@@ -110,6 +110,8 @@ enum {
PARAM_OFFSET=0,
};
#define MULTICAM_OFFSET 10 // 10 means max 10 cameras per pose
namespace rtabmap {
bool OptimizerG2O::available()
@@ -1395,7 +1397,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
int rootId,
const std::map<int, Transform> & poses,
const std::multimap<int, Link> & links,
const std::map<int, CameraModel> & models,
const std::map<int, std::vector<CameraModel> > & models,
std::map<int, cv::Point3f> & points3DMap,
const std::map<int, std::map<int, FeatureBA> > & wordReferences,
std::set<int> * outliers)
@@ -1488,50 +1490,55 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
if(iter->first > 0)
{
// Get camera model
std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && !iterModel->second.empty());
for(size_t i=0; i<iterModel->second.size(); ++i)
{
UASSERT(iterModel->second[i].isValidForProjection());
Transform camPose = iter->second * iterModel->second.localTransform();
Transform camPose = iter->second * iterModel->second[i].localTransform();
// Add node's pose
UASSERT(!camPose.isNull());
// Add node's pose
UASSERT(!camPose.isNull());
#ifdef RTABMAP_ORB_SLAM
g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
g2o::VertexSE3Expmap * vCam = new g2o::VertexSE3Expmap();
#else
g2o::VertexCam * vCam = new g2o::VertexCam();
g2o::VertexCam * vCam = new g2o::VertexCam();
#endif
Eigen::Affine3d a = camPose.toEigen3d();
Eigen::Affine3d a = camPose.toEigen3d();
#ifdef RTABMAP_ORB_SLAM
a = a.inverse();
vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
a = a.inverse();
vCam->setEstimate(g2o::SE3Quat(a.linear(), a.translation()));
#else
g2o::SBACam cam(Eigen::Quaterniond(a.linear()), a.translation());
cam.setKcam(
iterModel->second.fx(),
iterModel->second.fy(),
iterModel->second.cx(),
iterModel->second.cy(),
iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_); // baseline in meters
vCam->setEstimate(cam);
g2o::SBACam cam(Eigen::Quaterniond(a.linear()), a.translation());
cam.setKcam(
iterModel->second[i].fx(),
iterModel->second[i].fy(),
iterModel->second[i].cx(),
iterModel->second[i].cy(),
iterModel->second[i].Tx()<0.0?-iterModel->second[i].Tx()/iterModel->second[i].fx():baseline_); // baseline in meters
vCam->setEstimate(cam);
#endif
vCam->setId(iter->first);
vCam->setId(iter->first*MULTICAM_OFFSET + i);
// negative root means that all other poses should be fixed instead of the root
vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
// negative root means that all other poses should be fixed instead of the root
vCam->setFixed((rootId >= 0 && iter->first == rootId) || (rootId < 0 && iter->first != -rootId));
/*UDEBUG("cam %d (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
iter->first,
vCam->fixed()?1:0,
iterModel->second.fx(),
iterModel->second.fy(),
iterModel->second.cx(),
iterModel->second.cy(),
iterModel->second.Tx(),
iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_,
camPose.prettyPrint().c_str());*/
/*UDEBUG("camPose %d (camid=%d) (fixed=%d) fx=%f fy=%f cx=%f cy=%f Tx=%f baseline=%f t=%s",
iter->first,
vCam->id(),
vCam->fixed()?1:0,
iterModel->second[i].fx(),
iterModel->second[i].fy(),
iterModel->second[i].cx(),
iterModel->second[i].cy(),
iterModel->second[i].Tx(),
iterModel->second[i].Tx()<0.0?-iterModel->second[i].Tx()/iterModel->second[i].fx():baseline_,
camPose.prettyPrint().c_str());*/
UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert vertex %d!?", iter->first).c_str());
UASSERT_MSG(optimizer.addVertex(vCam), uFormat("cannot insert cam vertex %d (pose=%d)!?", vCam->id(), iter->first).c_str());
}
}
}
@@ -1563,11 +1570,12 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
Eigen::MatrixXd information = Eigen::MatrixXd::Identity(3, 3) * 1.0/(gravitySigma()*gravitySigma());
g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET);
EdgeSBACamGravity* priorEdge(new EdgeSBACamGravity());
std::map<int, CameraModel>::const_iterator iterModel = models.find(iter->first);
UASSERT(iterModel != models.end() && !iterModel->second.localTransform().isNull());
priorEdge->setCameraInvLocalTransform(iterModel->second.localTransform().inverse().toEigen3d().linear());
std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(iter->first);
// Gravity constraint added only to first camera of a pose
UASSERT(iterModel != models.end() && !iterModel->second.empty() && !iterModel->second[0].localTransform().isNull());
priorEdge->setCameraInvLocalTransform(iterModel->second[0].localTransform().inverse().toEigen3d().linear());
priorEdge->setMeasurement(m);
priorEdge->setInformation(information);
priorEdge->vertices()[0] = v1;
@@ -1592,15 +1600,17 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
}
// between cameras, not base_link
Transform camLink = models.at(id1).localTransform().inverse()*iter->second.transform()*models.at(id2).localTransform();
//UDEBUG("added edge %d->%d (in cam frame=%s)",
// id1,
// id2,
// camLink.prettyPrint().c_str());
Transform camLink = models.at(id1)[0].localTransform().inverse()*iter->second.transform()*models.at(id2)[0].localTransform();
/*UDEBUG("added edge %d->%d (camIDs %d->%d) (in cam frame=%s)",
id1,
id2,
id1*MULTICAM_OFFSET,
id2*MULTICAM_OFFSET,
camLink.prettyPrint().c_str());*/
#ifdef RTABMAP_ORB_SLAM
EdgeSE3Expmap * e = new EdgeSE3Expmap();
g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id1);
g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id2);
g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id1*MULTICAM_OFFSET);
g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id2*MULTICAM_OFFSET);
Transform camPose1 = Transform::fromEigen3d(v1->estimate()).inverse();
Transform camPose2Inv = Transform::fromEigen3d(v2->estimate());
@@ -1608,8 +1618,8 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
camLink = camPose1 * camPose1 * camLink * camPose2Inv * camPose2Inv;
#else
g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1);
g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2);
g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id1*MULTICAM_OFFSET);
g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id2*MULTICAM_OFFSET);
#endif
UASSERT(v1 != 0);
UASSERT(v2 != 0);
@@ -1629,8 +1639,60 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
}
}
UDEBUG("fill hard edges between camera 0 and other cameras (multicam)...");
for(std::map<int, std::vector<CameraModel> >::const_iterator iter=models.begin(); iter!=models.end(); ++iter)
{
int id = iter->first;
if(uContains(poses, id))
{
for(size_t i=1; i<iter->second.size(); ++i)
{
// add edge
// Set large information matrix to keep these links fixed
Eigen::Matrix<double, 6, 6> information = Eigen::Matrix<double, 6, 6>::Identity()*9999999;
// between cameras, not base_link
Transform camLink = iter->second[0].localTransform().inverse()*iter->second[i].localTransform();
#ifdef RTABMAP_ORB_SLAM
EdgeSE3Expmap * e = new EdgeSE3Expmap();
g2o::VertexSE3Expmap* v1 = (g2o::VertexSE3Expmap*)optimizer.vertex(id*MULTICAM_OFFSET);
g2o::VertexSE3Expmap* v2 = (g2o::VertexSE3Expmap*)optimizer.vertex(id*MULTICAM_OFFSET+i);
Transform camPose1 = Transform::fromEigen3d(v1->estimate()).inverse();
Transform camPose2Inv = Transform::fromEigen3d(v2->estimate());
camLink = camPose1 * camPose1 * camLink * camPose2Inv * camPose2Inv;
#else
g2o::EdgeSBACam * e = new g2o::EdgeSBACam();
g2o::VertexCam* v1 = (g2o::VertexCam*)optimizer.vertex(id*MULTICAM_OFFSET);
g2o::VertexCam* v2 = (g2o::VertexCam*)optimizer.vertex(id*MULTICAM_OFFSET+i);
#endif
/*UDEBUG("added edge between subcam 0->%d id:%d->%d (in cam frame=%s)",
i,
v1->id(),
v2->id(),
camLink.prettyPrint().c_str());*/
UASSERT(v1 != 0);
UASSERT(v2 != 0);
e->setVertex(0, v1);
e->setVertex(1, v2);
Eigen::Affine3d a = camLink.toEigen3d();
e->setMeasurement(g2o::SE3Quat(a.linear(), a.translation()));
e->setInformation(information);
if (!optimizer.addEdge(e))
{
delete e;
UERROR("Map: Failed adding constraint between %d and %d, skipping", v1->id(), v2->id());
return optimizedPoses;
}
}
}
}
UDEBUG("fill 3D points to g2o...");
const int stepVertexId = poses.rbegin()->first+1;
const int stepVertexId = poses.rbegin()->first*MULTICAM_OFFSET+MULTICAM_OFFSET;
int negVertexOffset = stepVertexId;
if(wordReferences.size() && wordReferences.rbegin()->first>0)
{
@@ -1669,22 +1731,24 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
// set observations
for(std::map<int, FeatureBA>::const_iterator jter=iter->second.begin(); jter!=iter->second.end(); ++jter)
{
int camId = jter->first;
if(poses.find(camId) != poses.end() && optimizer.vertex(camId) != 0)
int poseId = jter->first;
int camIndex = jter->second.cameraIndex;
int camId = poseId*MULTICAM_OFFSET+camIndex;
if(poses.find(poseId) != poses.end() && optimizer.vertex(camId) != 0)
{
const FeatureBA & pt = jter->second;
double depth = pt.depth;
//UDEBUG("Added observation pt=%d to cam=%d (%d,%d) depth=%f", vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth);
//UDEBUG("Added observation pt=%d to cam=%d (%d,%d) depth=%f (camIndex=%d)", vpt3d->id()-stepVertexId, camId, (int)pt.kpt.pt.x, (int)pt.kpt.pt.y, depth, camIndex);
g2o::OptimizableGraph::Edge * e;
double baseline = 0.0;
#ifdef RTABMAP_ORB_SLAM
g2o::VertexSE3Expmap* vcam = dynamic_cast<g2o::VertexSE3Expmap*>(optimizer.vertex(camId));
std::map<int, CameraModel>::const_iterator iterModel = models.find(camId);
std::map<int, std::vector<CameraModel> >::const_iterator iterModel = models.find(poseId);
UASSERT(iterModel != models.end() && iterModel->second.isValidForProjection());
baseline = iterModel->second.Tx()<0.0?-iterModel->second.Tx()/iterModel->second.fx():baseline_;
UASSERT(iterModel != models.end() && camIndex<iterModel->second.size() && iterModel->second[camIndex].isValidForProjection());
baseline = iterModel->second[camIndex].Tx()<0.0?-iterModel->second[camIndex].Tx()/iterModel->second[camIndex].fx():baseline_;
#else
g2o::VertexCam* vcam = dynamic_cast<g2o::VertexCam*>(optimizer.vertex(camId));
baseline = vcam->estimate().baseline;
@@ -1695,15 +1759,15 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
// stereo edge
#ifdef RTABMAP_ORB_SLAM
g2o::EdgeStereoSE3ProjectXYZ* es = new g2o::EdgeStereoSE3ProjectXYZ();
float disparity = baseline * iterModel->second.fx() / depth;
float disparity = baseline * iterModel->second[camIndex].fx() / depth;
Eigen::Vector3d obs( pt.kpt.pt.x, pt.kpt.pt.y, pt.kpt.pt.x-disparity);
es->setMeasurement(obs);
//variance *= log(exp(1)+disparity);
es->setInformation(Eigen::Matrix3d::Identity() / variance);
es->fx = iterModel->second.fx();
es->fy = iterModel->second.fy();
es->cx = iterModel->second.cx();
es->cy = iterModel->second.cy();
es->fx = iterModel->second[camIndex].fx();
es->fy = iterModel->second[camIndex].fy();
es->cx = iterModel->second[camIndex].cx();
es->cy = iterModel->second[camIndex].cy();
es->bf = baseline*es->fx;
e = es;
#else
@@ -1731,10 +1795,10 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
Eigen::Vector2d obs( pt.kpt.pt.x, pt.kpt.pt.y);
em->setMeasurement(obs);
em->setInformation(Eigen::Matrix2d::Identity() / variance);
em->fx = iterModel->second.fx();
em->fy = iterModel->second.fy();
em->cx = iterModel->second.cx();
em->cy = iterModel->second.cy();
em->fx = iterModel->second[camIndex].fx();
em->fy = iterModel->second[camIndex].fy();
em->cx = iterModel->second[camIndex].cx();
em->cy = iterModel->second[camIndex].cy();
e = em;
#else
@@ -1809,13 +1873,13 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
{
d = ((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[0]-((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->measurement()[2];
}
UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
//UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeStereoSE3ProjectXYZ*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
#else
if(dynamic_cast<g2o::EdgeProjectP2SC*>(*iter) != 0)
{
d = ((g2o::EdgeProjectP2SC*)(*iter))->measurement()[0]-((g2o::EdgeProjectP2SC*)(*iter))->measurement()[2];
}
UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
//UDEBUG("Ignoring edge (%d<->%d) d=%f var=%f kernel=%f chi2=%f", (*iter)->vertex(0)->id()-stepVertexId, (*iter)->vertex(1)->id(), d, 1.0/((g2o::EdgeProjectP2SC*)(*iter))->information()(0,0), (*iter)->robustKernel()->delta(), (*iter)->chi2());
#endif
cv::Point3f pt3d;
@@ -1858,10 +1922,11 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
{
if(iter->first > 0)
{
int camId = iter->first*MULTICAM_OFFSET;
#ifdef RTABMAP_ORB_SLAM
const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(iter->first);
const g2o::VertexSE3Expmap* v = (const g2o::VertexSE3Expmap*)optimizer.vertex(camId);
#else
const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(iter->first);
const g2o::VertexCam* v = (const g2o::VertexCam*)optimizer.vertex(camId);
#endif
if(v)
{
@@ -1872,7 +1937,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
#endif
// remove model local transform
t *= models.at(iter->first).localTransform().inverse();
t *= models.at(iter->first)[0].localTransform().inverse();
//UDEBUG("%d from=%s to=%s", iter->first, iter->second.prettyPrint().c_str(), t.prettyPrint().c_str());
if(t.isNull())
@@ -1896,7 +1961,7 @@ std::map<int, Transform> OptimizerG2O::optimizeBA(
}
else
{
UERROR("Vertex (pose) %d not found!?", iter->first);
UERROR("Vertex (pose) %d (cam=%d) not found!?", iter->first, camId);
}
}
}

239
corelib/src/util3d.cpp
View File

@@ -953,18 +953,8 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
UERROR("Camera model %d is invalid", i);
}
}
if(cloud->is_dense && validIndices)
{
//generate indices for all points (they are all valid)
validIndices->resize(cloud->size());
for(unsigned int i=0; i<cloud->size(); ++i)
{
validIndices->at(i) = i;
}
}
}
else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection())
else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && !sensorData.stereoCameraModels().empty())
{
//stereo
UASSERT(sensorData.rightRaw().type() == CV_8UC1);
@@ -979,51 +969,85 @@ pcl::PointCloud<pcl::PointXYZ>::Ptr RTABMAP_EXP cloudFromSensorData(
leftMono = sensorData.imageRaw();
}
cv::Mat right(sensorData.rightRaw());
StereoCameraModel model = sensorData.stereoCameraModel();
if( roiRatios.size() == 4 &&
((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
(roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
(roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
(roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
UASSERT(int((sensorData.imageRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.imageRaw().cols);
UASSERT(int((sensorData.rightRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.rightRaw().cols);
int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size();
for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
{
cv::Rect roi = util2d::computeRoi(leftMono, roiRatios);
if( roi.width%decimation==0 &&
roi.height%decimation==0)
if(sensorData.stereoCameraModels()[i].isValidForProjection())
{
leftMono = cv::Mat(leftMono, roi);
right = cv::Mat(right, roi);
model.roi(roi);
cv::Mat left(leftMono, cv::Rect(subImageWidth*i, 0, subImageWidth, leftMono.rows));
cv::Mat right(sensorData.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.rightRaw().rows));
StereoCameraModel model = sensorData.stereoCameraModels()[i];
if( roiRatios.size() == 4 &&
((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
(roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
(roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
(roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
{
cv::Rect roi = util2d::computeRoi(left, roiRatios);
if( roi.width%decimation==0 &&
roi.height%decimation==0)
{
left = cv::Mat(left, roi);
right = cv::Mat(right, roi);
model.roi(roi);
}
else
{
UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
"dimension (left=%dx%d) cannot be divided exactly "
"by decimation parameter (%d). Ignoring ROI ratios...",
roiRatios[0],
roiRatios[1],
roiRatios[2],
roiRatios[3],
roi.width,
roi.height,
decimation);
}
}
pcl::PointCloud<pcl::PointXYZ>::Ptr tmp = cloudFromDisparity(
util2d::disparityFromStereoImages(left, right, stereoParameters),
model,
decimation,
maxDepth,
minDepth,
validIndices);
if(tmp->size())
{
if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
{
tmp = util3d::transformPointCloud(tmp, model.localTransform());
}
if(sensorData.stereoCameraModels().size() > 1)
{
tmp = util3d::removeNaNFromPointCloud(tmp);
*cloud += *tmp;
}
else
{
cloud = tmp;
}
}
}
else
{
UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
"dimension (left=%dx%d) cannot be divided exactly "
"by decimation parameter (%d). Ignoring ROI ratios...",
roiRatios[0],
roiRatios[1],
roiRatios[2],
roiRatios[3],
roi.width,
roi.height,
decimation);
UERROR("Stereo camera model %d is invalid", i);
}
}
}
cloud = cloudFromDisparity(
util2d::disparityFromStereoImages(leftMono, right, stereoParameters),
model,
decimation,
maxDepth,
minDepth,
validIndices);
if(cloud->size())
if(!cloud->empty() && cloud->is_dense && validIndices)
{
//generate indices for all points (they are all valid)
validIndices->resize(cloud->size());
for(unsigned int i=0; i<cloud->size(); ++i)
{
if(cloud->size() && !sensorData.stereoCameraModel().left().localTransform().isNull() && !sensorData.stereoCameraModel().left().localTransform().isIdentity())
{
cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
}
validIndices->at(i) = i;
}
}
return cloud;
@@ -1129,69 +1153,96 @@ pcl::PointCloud<pcl::PointXYZRGB>::Ptr RTABMAP_EXP cloudRGBFromSensorData(
UERROR("Camera model %d is invalid", i);
}
}
if(cloud->is_dense && validIndices)
{
//generate indices for all points (they are all valid)
validIndices->resize(cloud->size());
for(unsigned int i=0; i<cloud->size(); ++i)
{
validIndices->at(i) = i;
}
}
}
else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && sensorData.stereoCameraModel().isValidForProjection())
else if(!sensorData.imageRaw().empty() && !sensorData.rightRaw().empty() && !sensorData.stereoCameraModels().empty())
{
//stereo
UDEBUG("");
cv::Mat left(sensorData.imageRaw());
cv::Mat right(sensorData.rightRaw());
StereoCameraModel model = sensorData.stereoCameraModel();
if( roiRatios.size() == 4 &&
((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
(roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
(roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
(roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
UASSERT(int((sensorData.imageRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.imageRaw().cols);
UASSERT(int((sensorData.rightRaw().cols/sensorData.stereoCameraModels().size())*sensorData.stereoCameraModels().size()) == sensorData.rightRaw().cols);
int subImageWidth = sensorData.rightRaw().cols/sensorData.stereoCameraModels().size();
for(unsigned int i=0; i<sensorData.stereoCameraModels().size(); ++i)
{
cv::Rect roi = util2d::computeRoi(left, roiRatios);
if( roi.width%decimation==0 &&
roi.height%decimation==0)
if(sensorData.stereoCameraModels()[i].isValidForProjection())
{
left = cv::Mat(left, roi);
right = cv::Mat(right, roi);
model.roi(roi);
cv::Mat left(sensorData.imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.imageRaw().rows));
cv::Mat right(sensorData.rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, sensorData.rightRaw().rows));
StereoCameraModel model = sensorData.stereoCameraModels()[i];
if( roiRatios.size() == 4 &&
((roiRatios[0] > 0.0f && roiRatios[0] <= 1.0f) ||
(roiRatios[1] > 0.0f && roiRatios[1] <= 1.0f) ||
(roiRatios[2] > 0.0f && roiRatios[2] <= 1.0f) ||
(roiRatios[3] > 0.0f && roiRatios[3] <= 1.0f)))
{
cv::Rect roi = util2d::computeRoi(left, roiRatios);
if( roi.width%decimation==0 &&
roi.height%decimation==0)
{
left = cv::Mat(left, roi);
right = cv::Mat(right, roi);
model.roi(roi);
}
else
{
UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
"dimension (left=%dx%d) cannot be divided exactly "
"by decimation parameter (%d). Ignoring ROI ratios...",
roiRatios[0],
roiRatios[1],
roiRatios[2],
roiRatios[3],
roi.width,
roi.height,
decimation);
}
}
pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = cloudFromStereoImages(
left,
right,
model,
decimation,
maxDepth,
minDepth,
validIndices,
stereoParameters);
if(tmp->size())
{
if(!model.localTransform().isNull() && !model.localTransform().isIdentity())
{
tmp = util3d::transformPointCloud(tmp, model.localTransform());
}
if(sensorData.stereoCameraModels().size() > 1)
{
tmp = util3d::removeNaNFromPointCloud(tmp);
*cloud += *tmp;
}
else
{
cloud = tmp;
}
}
}
else
{
UERROR("Cannot apply ROI ratios [%f,%f,%f,%f] because resulting "
"dimension (left=%dx%d) cannot be divided exactly "
"by decimation parameter (%d). Ignoring ROI ratios...",
roiRatios[0],
roiRatios[1],
roiRatios[2],
roiRatios[3],
roi.width,
roi.height,
decimation);
UERROR("Stereo camera model %d is invalid", i);
}
}
}
cloud = cloudFromStereoImages(
left,
right,
model,
decimation,
maxDepth,
minDepth,
validIndices,
stereoParameters);
if(cloud->size() && !sensorData.stereoCameraModel().left().localTransform().isNull() && !sensorData.stereoCameraModel().left().localTransform().isIdentity())
if(cloud->is_dense && validIndices)
{
//generate indices for all points (they are all valid)
validIndices->resize(cloud->size());
for(unsigned int i=0; i<cloud->size(); ++i)
{
cloud = util3d::transformPointCloud(cloud, sensorData.stereoCameraModel().left().localTransform());
validIndices->at(i) = i;
}
}
return cloud;
}

240
corelib/src/util3d_motion_estimation.cpp
View File

@@ -39,6 +39,13 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include "opencv/solvepnp.h"
#ifdef RTABMAP_OPENGV
#include <opengv/absolute_pose/methods.hpp>
#include <opengv/absolute_pose/NoncentralAbsoluteAdapter.hpp>
#include <opengv/sac/Ransac.hpp>
#include <opengv/sac_problems/absolute_pose/AbsolutePoseSacProblem.hpp>
#endif
namespace rtabmap
{
@@ -109,9 +116,9 @@ Transform estimateMotion3DTo2D(
(double)guessCameraFrame.r21(), (double)guessCameraFrame.r22(), (double)guessCameraFrame.r23(),
(double)guessCameraFrame.r31(), (double)guessCameraFrame.r32(), (double)guessCameraFrame.r33());
cv::Mat rvec(1,3, CV_64FC1);
cv::Mat rvec(3,1, CV_64FC1);
cv::Rodrigues(R, rvec);
cv::Mat tvec = (cv::Mat_<double>(1,3) <<
cv::Mat tvec = (cv::Mat_<double>(3,1) <<
(double)guessCameraFrame.x(), (double)guessCameraFrame.y(), (double)guessCameraFrame.z());
util3d::solvePnPRansac(
@@ -240,6 +247,235 @@ Transform estimateMotion3DTo2D(
return transform;
}
Transform estimateMotion3DTo2D(
const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::KeyPoint> & words2B,
const std::vector<CameraModel> & cameraModels,
int minInliers,
int iterations,
double reprojError,
int flagsPnP,
int refineIterations,
const Transform & guess,
const std::map<int, cv::Point3f> & words3B,
cv::Mat * covariance,
std::vector<int> * matchesOut,
std::vector<int> * inliersOut)
{
Transform transform;
#ifndef RTABMAP_OPENGV
UERROR("This function is only available if rtabmap is built with OpenGV dependency.");
#else
UASSERT(!cameraModels.empty() && cameraModels[0].imageWidth() > 0);
int subImageWidth = cameraModels[0].imageWidth();
for(size_t i=0; i<cameraModels.size(); ++i)
{
UASSERT(cameraModels[i].isValidForProjection());
UASSERT(subImageWidth == cameraModels[i].imageWidth());
}
UASSERT(!guess.isNull());
std::vector<int> matches, inliers;
if(covariance)
{
*covariance = cv::Mat::eye(6,6,CV_64FC1);
}
// find correspondences
std::vector<int> ids = uKeys(words2B);
std::vector<cv::Point3f> objectPoints(ids.size());
std::vector<cv::Point2f> imagePoints(ids.size());
int oi=0;
matches.resize(ids.size());
std::vector<int> cameraIndexes(ids.size());
for(unsigned int i=0; i<ids.size(); ++i)
{
std::map<int, cv::Point3f>::const_iterator iter=words3A.find(ids[i]);
if(iter != words3A.end() && util3d::isFinite(iter->second))
{
const cv::Point2f & kpt = words2B.find(ids[i])->second.pt;
int cameraIndex = int(kpt.x / subImageWidth);
UASSERT_MSG(cameraIndex >= 0 && cameraIndex < (int)cameraModels.size(),
uFormat("cameraIndex=%d, models=%d, kpt.x=%f, subImageWidth=%f (Camera model image width=%d)",
cameraIndex, (int)cameraModels.size(), kpt.x, subImageWidth, cameraModels[cameraIndex].imageWidth()).c_str());
const cv::Point3f & pt = iter->second;
objectPoints[oi] = pt;
imagePoints[oi] = kpt;
// convert in image space
imagePoints[oi].x = imagePoints[oi].x - (cameraIndex*subImageWidth);
cameraIndexes[oi] = cameraIndex;
matches[oi++] = ids[i];
}
}
objectPoints.resize(oi);
imagePoints.resize(oi);
cameraIndexes.resize(oi);
matches.resize(oi);
UDEBUG("words3A=%d words2B=%d matches=%d words3B=%d guess=%s reprojError=%f iterations=%d",
(int)words3A.size(), (int)words2B.size(), (int)matches.size(), (int)words3B.size(),
guess.prettyPrint().c_str(), reprojError, iterations);
if((int)matches.size() >= minInliers)
{
// convert cameras
opengv::translations_t camOffsets;
opengv::rotations_t camRotations;
for(size_t i=0; i<cameraModels.size(); ++i)
{
camOffsets.push_back(opengv::translation_t(
cameraModels[i].localTransform().x(),
cameraModels[i].localTransform().y(),
cameraModels[i].localTransform().z()));
camRotations.push_back(cameraModels[i].localTransform().toEigen4d().block<3,3>(0, 0));
}
// convert 3d points
opengv::points_t points;
// convert 2d-3d correspondences into bearing vectors
opengv::bearingVectors_t bearingVectors;
opengv::absolute_pose::NoncentralAbsoluteAdapter::camCorrespondences_t camCorrespondences;
for(size_t i=0; i<objectPoints.size(); ++i)
{
int cameraIndex = cameraIndexes[i];
points.push_back(opengv::point_t(objectPoints[i].x,objectPoints[i].y,objectPoints[i].z));
cv::Vec3f pt;
cameraModels[cameraIndex].project(imagePoints[i].x, imagePoints[i].y, 1, pt[0], pt[1], pt[2]);
pt = cv::normalize(pt);
bearingVectors.push_back(opengv::bearingVector_t(pt[0], pt[1], pt[2]));
camCorrespondences.push_back(cameraIndex);
}
//create a non-central absolute adapter
opengv::absolute_pose::NoncentralAbsoluteAdapter adapter(
bearingVectors,
camCorrespondences,
points,
camOffsets,
camRotations );
adapter.setR(guess.toEigen4d().block<3,3>(0, 0));
adapter.sett(opengv::translation_t(guess.x(), guess.y(), guess.z()));
//Create a AbsolutePoseSacProblem and Ransac
//The method is set to GP3P
opengv::sac::Ransac<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> ransac;
std::shared_ptr<opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem> absposeproblem_ptr(
new opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem(adapter, opengv::sac_problems::absolute_pose::AbsolutePoseSacProblem::GP3P));
ransac.sac_model_ = absposeproblem_ptr;
ransac.threshold_ = 1.0 - cos(atan(reprojError/cameraModels[0].fx()));
ransac.max_iterations_ = iterations;
UDEBUG("Ransac params: threshold = %f (reprojError=%f fx=%f), max iterations=%d", ransac.threshold_, reprojError, cameraModels[0].fx(), ransac.max_iterations_);
//Run the experiment
ransac.computeModel();
Transform pnp = Transform::fromEigen3d(ransac.model_coefficients_);
UDEBUG("Ransac result: %s", pnp.prettyPrint().c_str());
UDEBUG("Ransac iterations done: %d", ransac.iterations_);
inliers = ransac.inliers_;
UDEBUG("Ransac inliers: %ld", inliers.size());
if((int)inliers.size() >= minInliers)
{
transform = pnp;
// compute variance (like in PCL computeVariance() method of sac_model.h)
if(covariance)
{
std::vector<float> errorSqrdDists(inliers.size());
std::vector<float> errorSqrdAngles(inliers.size());
oi = 0;
for(unsigned int i=0; i<inliers.size(); ++i)
{
std::map<int, cv::Point3f>::const_iterator iter = words3B.find(matches[inliers[i]]);
if(words3B.empty() || (iter != words3B.end() && util3d::isFinite(iter->second)))
{
const cv::Point3f & objPt = objectPoints[inliers[i]];
cv::Point3f newPt;
if(iter!=words3B.end())
{
newPt = util3d::transformPoint(iter->second, transform);
}
else
{
//compute from projection
int cameraIndex = cameraIndexes[inliers[i]];
Transform transformCameraFrame = transform * cameraModels[cameraIndex].localTransform();
Transform transformCameraFrameInv = transformCameraFrame.inverse();
Eigen::Vector3f ray = projectDepthTo3DRay(
cameraModels[cameraIndex].imageSize(),
imagePoints.at(inliers[i]).x,
imagePoints.at(inliers[i]).y,
cameraModels[cameraIndex].cx(),
cameraModels[cameraIndex].cy(),
cameraModels[cameraIndex].fx(),
cameraModels[cameraIndex].fy());
// transform in camera B frame
newPt = util3d::transformPoint(objPt, transformCameraFrameInv);
newPt = cv::Point3f(ray.x(), ray.y(), ray.z()) * newPt.z*1.1; // Add 10 % error
// put back in frame of camera A
newPt = util3d::transformPoint(newPt, transformCameraFrame);
}
errorSqrdDists[oi] = uNormSquared(objPt.x-newPt.x, objPt.y-newPt.y, objPt.z-newPt.z);
Eigen::Vector4f v1(objPt.x - transform.x(), objPt.y - transform.y(), objPt.z - transform.z(), 0);
Eigen::Vector4f v2(newPt.x - transform.x(), newPt.y - transform.y(), newPt.z - transform.z(), 0);
errorSqrdAngles[oi++] = pcl::getAngle3D(v1, v2);
}
}
errorSqrdDists.resize(oi);
errorSqrdAngles.resize(oi);
if(errorSqrdDists.size())
{
std::sort(errorSqrdDists.begin(), errorSqrdDists.end());
//divide by 4 instead of 2 to ignore very very far features (stereo)
double median_error_sqr = 2.1981 * (double)errorSqrdDists[errorSqrdDists.size () >> 2];
UASSERT(uIsFinite(median_error_sqr));
(*covariance)(cv::Range(0,3), cv::Range(0,3)) *= median_error_sqr;
std::sort(errorSqrdAngles.begin(), errorSqrdAngles.end());
median_error_sqr = 2.1981 * (double)errorSqrdAngles[errorSqrdAngles.size () >> 2];
UASSERT(uIsFinite(median_error_sqr));
(*covariance)(cv::Range(3,6), cv::Range(3,6)) *= median_error_sqr;
}
else
{
UWARN("Not enough close points to compute covariance!");
}
if(float(oi) / float(inliers.size()) < 0.2f)
{
UWARN("A very low number of inliers have valid depth (%d/%d), the transform returned may be wrong!", oi, (int)inliers.size());
}
}
}
}
if(matchesOut)
{
*matchesOut = matches;
}
if(inliersOut)
{
inliersOut->resize(inliers.size());
for(unsigned int i=0; i<inliers.size(); ++i)
{
inliersOut->at(i) = matches[inliers[i]];
}
}
#endif
return transform;
}
Transform estimateMotion3DTo3D(
const std::map<int, cv::Point3f> & words3A,
const std::map<int, cv::Point3f> & words3B,

58
corelib/src/util3d_surface.cpp
View File

@@ -1568,18 +1568,19 @@ cv::Mat mergeTextures(
else if(memory)
{
SensorData data = memory->getNodeData(textureId, true, false, false, false);
std::vector<CameraModel> models = data.cameraModels();
StereoCameraModel stereoModel = data.stereoCameraModel();
const std::vector<CameraModel> & models = data.cameraModels();
const std::vector<StereoCameraModel> & stereoModels = data.stereoCameraModels();
if(models.size()>=1 &&
models[0].imageHeight()>0 &&
models[0].imageWidth()>0)
{
imageSize = models[0].imageSize();
}
else if(stereoModel.left().imageHeight() > 0 &&
stereoModel.left().imageWidth() > 0)
else if(stereoModels.size()>=1 &&
stereoModels[0].left().imageHeight() > 0 &&
stereoModels[0].left().imageWidth() > 0)
{
imageSize = stereoModel.left().imageSize();
imageSize = stereoModels[0].left().imageSize();
}
else // backward compatibility for image size not set in CameraModel
{
@@ -1596,18 +1597,19 @@ cv::Mat mergeTextures(
else if(dbDriver)
{
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
dbDriver->getCalibration(textureId, models, stereoModel);
std::vector<StereoCameraModel> stereoModels;
dbDriver->getCalibration(textureId, models, stereoModels);
if(models.size()>=1 &&
models[0].imageHeight()>0 &&
models[0].imageWidth()>0)
{
imageSize = models[0].imageSize();
}
else if(stereoModel.left().imageHeight() > 0 &&
stereoModel.left().imageWidth() > 0)
else if(stereoModels.size()>=1 &&
stereoModels[0].left().imageHeight() > 0 &&
stereoModels[0].left().imageWidth() > 0)
{
imageSize = stereoModel.left().imageSize();
imageSize = stereoModels[0].left().imageSize();
}
else // backward compatibility for image size not set in CameraModel
{
@@ -1698,6 +1700,13 @@ cv::Mat mergeTextures(
{
SensorData data = memory->getNodeData(textures[t].first, true, false, false, false);
models = data.cameraModels();
if(models.empty() && !data.stereoCameraModels().empty())
{
for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
{
models.push_back(data.stereoCameraModels()[i].left());
}
}
data.uncompressDataConst(&image, 0);
}
else if(dbDriver)
@@ -1705,8 +1714,15 @@ cv::Mat mergeTextures(
SensorData data;
dbDriver->getNodeData(textures[t].first, data, true, false, false, false);
data.uncompressDataConst(&image, 0);
StereoCameraModel stereoModel;
dbDriver->getCalibration(textures[t].first, models, stereoModel);
std::vector<StereoCameraModel> stereoModels;
dbDriver->getCalibration(textures[t].first, models, stereoModels);
if(models.empty() && !stereoModels.empty())
{
for(size_t i=0; i<stereoModels.size(); ++i)
{
models.push_back(stereoModels[i].left());
}
}
}
previousImage = image;
@@ -2401,9 +2417,12 @@ bool multiBandTexturing(
{
SensorData data = memory->getNodeData(camId, true, false, false, false);
models = data.cameraModels();
if(models.empty() && data.stereoCameraModel().isValidForProjection())
if(models.empty() && data.stereoCameraModels().size())
{
models.push_back(data.stereoCameraModel().left());
for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
{
models.push_back(data.stereoCameraModels()[i].left());
}
}
if(data.imageRaw().empty())
{
@@ -2432,11 +2451,14 @@ bool multiBandTexturing(
}
else if(dbDriver)
{
StereoCameraModel stereoModel;
dbDriver->getCalibration(camId, models, stereoModel);
if(models.empty() && stereoModel.isValidForProjection())
std::vector<StereoCameraModel> stereoModels;
dbDriver->getCalibration(camId, models, stereoModels);
if(models.empty() && stereoModels.size())
{
models.push_back(stereoModel.left());
for(size_t i=0; i<stereoModels.size(); ++i)
{
models.push_back(stereoModels[i].left());
}
}
SensorData data;

10
corelib/src/util3d_transforms.cpp
View File

@@ -218,6 +218,16 @@ cv::Point3f transformPoint(
ret.z = transform (2, 0) * point.x + transform (2, 1) * point.y + transform (2, 2) * point.z + transform (2, 3);
return ret;
}
cv::Point3d transformPoint(
const cv::Point3d & point,
const Transform & transform)
{
cv::Point3d ret = point;
ret.x = transform (0, 0) * point.x + transform (0, 1) * point.y + transform (0, 2) * point.z + transform (0, 3);
ret.y = transform (1, 0) * point.x + transform (1, 1) * point.y + transform (1, 2) * point.z + transform (1, 3);
ret.z = transform (2, 0) * point.x + transform (2, 1) * point.y + transform (2, 2) * point.z + transform (2, 3);
return ret;
}
pcl::PointXYZ transformPoint(
const pcl::PointXYZ & pt,
const Transform & transform)