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

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

2
guilib/src/CameraViewer.cpp
View File

@@ -125,7 +125,7 @@ void CameraViewer::showImage(const rtabmap::SensorData & data)
imageSizeLabel_->setText(sizes);
if(!data.depthOrRightRaw().empty() &&
(data.stereoCameraModel().isValidForProjection() || (data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())))
((data.stereoCameraModels().size() && data.stereoCameraModels()[0].isValidForProjection()) || (data.cameraModels().size() && data.cameraModels().at(0).isValidForProjection())))
{
if(showCloudCheckbox_->isChecked())
{

15
guilib/src/CloudViewer.cpp
View File

@@ -3008,6 +3008,21 @@ void CloudViewer::updateCameraFrustums(const Transform & pose, const std::vector
}
}
}
void CloudViewer::updateCameraFrustums(const Transform & pose, const std::vector<StereoCameraModel> & stereoModels)
{
std::vector<CameraModel> models;
for(size_t i=0; i<stereoModels.size(); ++i)
{
models.push_back(stereoModels[i].left());
CameraModel right = stereoModels[i].right();
if(!stereoModels[i].left().localTransform().isNull())
{
right.setLocalTransform(stereoModels[i].left().localTransform() * Transform(stereoModels[i].baseline(), 0, 0, 0, 0, 0));
}
models.push_back(right);
updateCameraFrustums(pose, models);
}
}
const QColor & CloudViewer::getDefaultBackgroundColor() const
{

214
guilib/src/DatabaseViewer.cpp
View File

@@ -1398,7 +1398,7 @@ void DatabaseViewer::exportDatabase()
scan,
rgb,
depth,
data.stereoCameraModel(),
data.stereoCameraModels(),
id,
stamps.at(id),
userData);
@@ -1541,33 +1541,40 @@ void DatabaseViewer::extractImages()
{
UERROR("Cannot save calibration file, database name is empty!");
}
else if(data.stereoCameraModel().isValidForProjection())
else if(data.stereoCameraModels().size()>=1 && data.stereoCameraModels().front().isValidForProjection())
{
std::string cameraName = id.toStdString();
StereoCameraModel model(
cameraName,
data.imageRaw().size(),
data.stereoCameraModel().left().K(),
data.stereoCameraModel().left().D(),
data.stereoCameraModel().left().R(),
data.stereoCameraModel().left().P(),
data.rightRaw().size(),
data.stereoCameraModel().right().K(),
data.stereoCameraModel().right().D(),
data.stereoCameraModel().right().R(),
data.stereoCameraModel().right().P(),
data.stereoCameraModel().R(),
data.stereoCameraModel().T(),
data.stereoCameraModel().E(),
data.stereoCameraModel().F(),
data.stereoCameraModel().left().localTransform());
if(model.save(path.toStdString() + "/calib"))
for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
{
UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/calib/"+cameraName+".yaml").c_str());
}
else
{
UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/calib/"+cameraName+".yaml").c_str());
std::string cameraName = id.toStdString();
if(data.stereoCameraModels().size()>1)
{
cameraName+="_"+uNumber2Str((int)i);
}
StereoCameraModel model(
cameraName,
data.imageRaw().size(),
data.stereoCameraModels()[i].left().K(),
data.stereoCameraModels()[i].left().D(),
data.stereoCameraModels()[i].left().R(),
data.stereoCameraModels()[i].left().P(),
data.rightRaw().size(),
data.stereoCameraModels()[i].right().K(),
data.stereoCameraModels()[i].right().D(),
data.stereoCameraModels()[i].right().R(),
data.stereoCameraModels()[i].right().P(),
data.stereoCameraModels()[i].R(),
data.stereoCameraModels()[i].T(),
data.stereoCameraModels()[i].E(),
data.stereoCameraModels()[i].F(),
data.stereoCameraModels()[i].left().localTransform());
if(model.save(path.toStdString() + "/calib"))
{
UINFO("Saved stereo calibration \"%s\"", (path.toStdString()+"/calib/"+cameraName+".yaml").c_str());
}
else
{
UERROR("Failed saving calibration \"%s\"", (path.toStdString()+"/calib/"+cameraName+".yaml").c_str());
}
}
}
}
@@ -2614,17 +2621,18 @@ void DatabaseViewer::exportPoses(int format)
if(cameraFrame)
{
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
if(dbDriver_->getCalibration(iter->first, models, stereoModel))
std::vector<StereoCameraModel> stereoModels;
if(dbDriver_->getCalibration(iter->first, models, stereoModels))
{
if((models.size() == 1 &&
!models.at(0).localTransform().isNull()))
{
localTransform = models.at(0).localTransform();
}
else if(!stereoModel.localTransform().isNull())
else if(stereoModels.size() == 1 &&
!stereoModels[0].localTransform().isNull())
{
localTransform = stereoModel.localTransform();
localTransform = stereoModels[0].localTransform();
}
else if(models.size()>1)
{
@@ -3748,10 +3756,27 @@ void DatabaseViewer::regenerateLocalMaps()
viewpoint.z /= sum;
}
}
else
else if(s.sensorData().cameraModels().size())
{
const Transform & t = s.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<s.sensorData().stereoCameraModels().size(); ++i)
{
const Transform & t = s.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;
}
}
grid.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint);
@@ -3871,10 +3896,27 @@ void DatabaseViewer::regenerateCurrentLocalMaps()
viewpoint.z /= sum;
}
}
else
else if(s.sensorData().stereoCameraModels().size())
{
const Transform & t = s.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<s.sensorData().stereoCameraModels().size(); ++i)
{
const Transform & t = s.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;
}
}
grid.createLocalMap(util3d::laserScanFromPointCloud(*cloud), s.getPose(), ground, obstacles, empty, viewpoint);
@@ -4442,11 +4484,12 @@ void DatabaseViewer::update(int value,
}
if( !data.imageRaw().empty() &&
!data.rightRaw().empty() &&
data.stereoCameraModel().isValidForProjection() &&
data.stereoCameraModels().size()==1 && // Multiple stereo cameras not implemented
data.stereoCameraModels()[0].isValidForProjection() &&
ui_->checkBox_showDisparityInsteadOfRight->isChecked())
{
rtabmap::StereoDense * denseStereo = rtabmap::StereoDense::create(ui_->parameters_toolbox->getParameters());
depth = util2d::depthFromDisparity(denseStereo->computeDisparity(data.imageRaw(), data.rightRaw()), data.stereoCameraModel().left().fx(), data.stereoCameraModel().baseline(), CV_32FC1);
depth = util2d::depthFromDisparity(denseStereo->computeDisparity(data.imageRaw(), data.rightRaw()), data.stereoCameraModels()[0].left().fx(), data.stereoCameraModels()[0].baseline(), CV_32FC1);
delete denseStereo;
}
imgDepth = depth;
@@ -4591,7 +4634,7 @@ void DatabaseViewer::update(int value,
labelSensors->setText(sensorsStr);
labelSensors->setToolTip(tooltipStr);
}
if(data.cameraModels().size() || data.stereoCameraModel().isValidForProjection())
if(data.cameraModels().size() || data.stereoCameraModels().size())
{
std::stringstream calibrationDetails;
if(data.cameraModels().size())
@@ -4640,38 +4683,40 @@ void DatabaseViewer::update(int value,
}
}
else
else if(data.stereoCameraModels().size())
{
//stereo
labelCalib->setText(tr("%1x%2 fx=%3 fy=%4 cx=%5 cy=%6 baseline=%7m T=%8 [%9 %10 %11 %12; %13 %14 %15 %16; %17 %18 %19 %20]")
.arg(data.stereoCameraModel().left().imageWidth()>0?data.stereoCameraModel().left().imageWidth():data.imageRaw().cols)
.arg(data.stereoCameraModel().left().imageHeight()>0?data.stereoCameraModel().left().imageHeight():data.imageRaw().rows)
.arg(data.stereoCameraModel().left().fx())
.arg(data.stereoCameraModel().left().fy())
.arg(data.stereoCameraModel().left().cx())
.arg(data.stereoCameraModel().left().cy())
.arg(data.stereoCameraModel().baseline())
.arg(data.stereoCameraModel().localTransform().prettyPrint().c_str())
.arg(data.stereoCameraModel().localTransform().r11()).arg(data.stereoCameraModel().localTransform().r12()).arg(data.stereoCameraModel().localTransform().r13()).arg(data.stereoCameraModel().localTransform().o14())
.arg(data.stereoCameraModel().localTransform().r21()).arg(data.stereoCameraModel().localTransform().r22()).arg(data.stereoCameraModel().localTransform().r23()).arg(data.stereoCameraModel().localTransform().o24())
.arg(data.stereoCameraModel().localTransform().r31()).arg(data.stereoCameraModel().localTransform().r32()).arg(data.stereoCameraModel().localTransform().r33()).arg(data.stereoCameraModel().localTransform().o34()));
.arg(data.stereoCameraModels()[0].left().imageWidth()>0?data.stereoCameraModels()[0].left().imageWidth():data.imageRaw().cols)
.arg(data.stereoCameraModels()[0].left().imageHeight()>0?data.stereoCameraModels()[0].left().imageHeight():data.imageRaw().rows)
.arg(data.stereoCameraModels()[0].left().fx())
.arg(data.stereoCameraModels()[0].left().fy())
.arg(data.stereoCameraModels()[0].left().cx())
.arg(data.stereoCameraModels()[0].left().cy())
.arg(data.stereoCameraModels()[0].baseline())
.arg(data.stereoCameraModels()[0].localTransform().prettyPrint().c_str())
.arg(data.stereoCameraModels()[0].localTransform().r11()).arg(data.stereoCameraModels()[0].localTransform().r12()).arg(data.stereoCameraModels()[0].localTransform().r13()).arg(data.stereoCameraModels()[0].localTransform().o14())
.arg(data.stereoCameraModels()[0].localTransform().r21()).arg(data.stereoCameraModels()[0].localTransform().r22()).arg(data.stereoCameraModels()[0].localTransform().r23()).arg(data.stereoCameraModels()[0].localTransform().o24())
.arg(data.stereoCameraModels()[0].localTransform().r31()).arg(data.stereoCameraModels()[0].localTransform().r32()).arg(data.stereoCameraModels()[0].localTransform().r33()).arg(data.stereoCameraModels()[0].localTransform().o34()));
calibrationDetails << "Left:" << " Size=" << data.stereoCameraModel().left().imageWidth() << "x" << data.stereoCameraModel().left().imageHeight() << std::endl;
if( data.stereoCameraModel().left().K_raw().total()) calibrationDetails << "K=" << data.stereoCameraModel().left().K_raw() << std::endl;
if( data.stereoCameraModel().left().D_raw().total()) calibrationDetails << "D=" << data.stereoCameraModel().left().D_raw() << std::endl;
if( data.stereoCameraModel().left().R().total()) calibrationDetails << "R=" << data.stereoCameraModel().left().R() << std::endl;
if( data.stereoCameraModel().left().P().total()) calibrationDetails << "P=" << data.stereoCameraModel().left().P() << std::endl;
calibrationDetails << std::endl;
calibrationDetails << "Right:" << " Size=" << data.stereoCameraModel().right().imageWidth() << "x" << data.stereoCameraModel().right().imageHeight() << std::endl;
if( data.stereoCameraModel().right().K_raw().total()) calibrationDetails << "K=" << data.stereoCameraModel().right().K_raw() << std::endl;
if( data.stereoCameraModel().right().D_raw().total()) calibrationDetails << "D=" << data.stereoCameraModel().right().D_raw() << std::endl;
if( data.stereoCameraModel().right().R().total()) calibrationDetails << "R=" << data.stereoCameraModel().right().R() << std::endl;
if( data.stereoCameraModel().right().P().total()) calibrationDetails << "P=" << data.stereoCameraModel().right().P() << std::endl;
calibrationDetails << std::endl;
if( data.stereoCameraModel().R().total()) calibrationDetails << "R=" << data.stereoCameraModel().R() << std::endl;
if( data.stereoCameraModel().T().total()) calibrationDetails << "T=" << data.stereoCameraModel().T() << std::endl;
if( data.stereoCameraModel().F().total()) calibrationDetails << "F=" << data.stereoCameraModel().F() << std::endl;
if( data.stereoCameraModel().E().total()) calibrationDetails << "E=" << data.stereoCameraModel().E() << std::endl;
for(unsigned int i=0; i<data.stereoCameraModels().size();++i)
{
calibrationDetails << "Id: " << i << std::endl;
calibrationDetails << " Left:" << " Size=" << data.stereoCameraModels()[i].left().imageWidth() << "x" << data.stereoCameraModels()[i].left().imageHeight() << std::endl;
if( data.stereoCameraModels()[i].left().K_raw().total()) calibrationDetails << " K=" << data.stereoCameraModels()[i].left().K_raw() << std::endl;
if( data.stereoCameraModels()[i].left().D_raw().total()) calibrationDetails << " D=" << data.stereoCameraModels()[i].left().D_raw() << std::endl;
if( data.stereoCameraModels()[i].left().R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].left().R() << std::endl;
if( data.stereoCameraModels()[i].left().P().total()) calibrationDetails << " P=" << data.stereoCameraModels()[i].left().P() << std::endl;
calibrationDetails << " Right:" << " Size=" << data.stereoCameraModels()[i].right().imageWidth() << "x" << data.stereoCameraModels()[i].right().imageHeight() << std::endl;
if( data.stereoCameraModels()[i].right().K_raw().total()) calibrationDetails << " K=" << data.stereoCameraModels()[i].right().K_raw() << std::endl;
if( data.stereoCameraModels()[i].right().D_raw().total()) calibrationDetails << " D=" << data.stereoCameraModels()[i].right().D_raw() << std::endl;
if( data.stereoCameraModels()[i].right().R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].right().R() << std::endl;
if( data.stereoCameraModels()[i].right().P().total()) calibrationDetails << " P=" << data.stereoCameraModels()[i].right().P() << std::endl;
if( data.stereoCameraModels()[i].R().total()) calibrationDetails << " R=" << data.stereoCameraModels()[i].R() << std::endl;
if( data.stereoCameraModels()[i].T().total()) calibrationDetails << " T=" << data.stereoCameraModels()[i].T() << std::endl;
if( data.stereoCameraModels()[i].F().total()) calibrationDetails << " F=" << data.stereoCameraModels()[i].F() << std::endl;
if( data.stereoCameraModels()[i].E().total()) calibrationDetails << " E=" << data.stereoCameraModels()[i].E() << std::endl;
}
}
labelCalib->setToolTip(calibrationDetails.str().c_str());
@@ -4791,12 +4836,16 @@ void DatabaseViewer::update(int value,
{
pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud = util3d::laserScanToPointCloudINormal(laserScanRaw, laserScanRaw.localTransform());
std::vector<CameraModel> models = data.cameraModels();
if(data.stereoCameraModel().isValidForProjection())
if(!data.stereoCameraModels().empty())
{
models.clear();
models.push_back(data.stereoCameraModel().left());
for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
{
models.push_back(data.stereoCameraModels()[i].left());
}
}
else if(!models.empty() && !models[0].isValidForProjection())
if(!models.empty() && !models[0].isValidForProjection())
{
models.clear();
}
@@ -4922,11 +4971,11 @@ void DatabaseViewer::update(int value,
viewpoint[1] = data.cameraModels()[0].localTransform().y();
viewpoint[2] = data.cameraModels()[0].localTransform().z();
}
else if(!data.stereoCameraModel().localTransform().isNull())
else if(data.stereoCameraModels().size() && !data.stereoCameraModels()[0].localTransform().isNull())
{
viewpoint[0] = data.stereoCameraModel().localTransform().x();
viewpoint[1] = data.stereoCameraModel().localTransform().y();
viewpoint[2] = data.stereoCameraModel().localTransform().z();
viewpoint[0] = data.stereoCameraModels()[0].localTransform().x();
viewpoint[1] = data.stereoCameraModels()[0].localTransform().y();
viewpoint[2] = data.stereoCameraModels()[0].localTransform().z();
}
std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
cloud,
@@ -4995,7 +5044,7 @@ void DatabaseViewer::update(int value,
}
else
{
cloudViewer_->updateCameraFrustum(pose, data.stereoCameraModel());
cloudViewer_->updateCameraFrustums(pose, data.stereoCameraModels());
}
}
@@ -5365,7 +5414,8 @@ void DatabaseViewer::updateStereo(const SensorData * data)
!data->imageRaw().empty() &&
!data->depthOrRightRaw().empty() &&
data->depthOrRightRaw().type() == CV_8UC1 &&
data->stereoCameraModel().isValidForProjection())
data->stereoCameraModels().size()==1 && // Not implemented for multiple stereo cameras
data->stereoCameraModels()[0].isValidForProjection())
{
cv::Mat leftMono;
if(data->imageRaw().channels() == 3)
@@ -5433,11 +5483,11 @@ void DatabaseViewer::updateStereo(const SensorData * data)
cv::Point3f tmpPt = util3d::projectDisparityTo3D(
leftCorners[i],
disparity,
data->stereoCameraModel());
data->stereoCameraModels()[0]);
if(util3d::isFinite(tmpPt))
{
pt = util3d::transformPoint(tmpPt, data->stereoCameraModel().left().localTransform());
pt = util3d::transformPoint(tmpPt, data->stereoCameraModels()[0].left().localTransform());
status[i] = 100; //blue
++inliers;
cloud->at(oi++) = pcl::PointXYZ(pt.x, pt.y, pt.z);
@@ -6403,9 +6453,9 @@ void DatabaseViewer::updateConstraintView(
{
model = dataFrom.cameraModels()[0];
}
else
else if(dataFrom.stereoCameraModels().size())
{
model = dataFrom.stereoCameraModel().left();
model = dataFrom.stereoCameraModels()[0].left();
}
constraintsViewer_->addOrUpdateFrustum("frustum_from", pose, model.localTransform(), constraintsViewer_->getFrustumScale(), constraintsViewer_->getFrustumColor(), model.fovX(), model.fovY());
model = CameraModel();
@@ -6413,9 +6463,9 @@ void DatabaseViewer::updateConstraintView(
{
model = dataTo.cameraModels()[0];
}
else
else if(dataTo.stereoCameraModels().size())
{
model = dataTo.stereoCameraModel().left();
model = dataTo.stereoCameraModels()[0].left();
}
constraintsViewer_->addOrUpdateFrustum("frustum_to", pose*t, model.localTransform(), constraintsViewer_->getFrustumScale(), constraintsViewer_->getFrustumColor(), model.fovX(), model.fovY());
}

12
guilib/src/ExportBundlerDialog.cpp
View File

@@ -445,10 +445,10 @@ void ExportBundlerDialog::exportBundler(
out << ster.value().sensorData().cameraModels().at(0).fx() << " 0 0\n";
localTransform = ster.value().sensorData().cameraModels().at(0).localTransform();
}
else
else if(ster.value().sensorData().stereoCameraModels().size())
{
out << ster.value().sensorData().stereoCameraModel().left().fx() << " 0 0\n";
localTransform = ster.value().sensorData().stereoCameraModel().left().localTransform();
out << ster.value().sensorData().stereoCameraModels()[0].left().fx() << " 0 0\n";
localTransform = ster.value().sensorData().stereoCameraModels()[0].left().localTransform();
}
Transform pose = iter->second;
@@ -519,10 +519,10 @@ void ExportBundlerDialog::exportBundler(
pt.x = kter->second.kpt.pt.x - s.sensorData().cameraModels().at(0).cx();
pt.y = kter->second.kpt.pt.y - s.sensorData().cameraModels().at(0).cy();
}
else
else if(signatures[camId].sensorData().stereoCameraModels().size())
{
pt.x = kter->second.kpt.pt.x - s.sensorData().stereoCameraModel().left().cx();
pt.y = kter->second.kpt.pt.y - s.sensorData().stereoCameraModel().left().cy();
pt.x = kter->second.kpt.pt.x - s.sensorData().stereoCameraModels()[0].left().cx();
pt.y = kter->second.kpt.pt.y - s.sensorData().stereoCameraModels()[0].left().cy();
}
descriptorIndexes.insert(std::make_pair(camId, 0));
out << " " << cameraIndexes.at(camId) << " " << descriptorIndexes.at(camId)++ << " " << pt.x << " " << -pt.y;

120
guilib/src/ExportCloudsDialog.cpp
View File

@@ -1256,9 +1256,12 @@ void ExportCloudsDialog::viewClouds(
{
models = iter->sensorData().cameraModels();
}
else if(iter->sensorData().stereoCameraModel().isValidForProjection())
else if(iter->sensorData().stereoCameraModels().size())
{
models.push_back(iter->sensorData().stereoCameraModel().left());
for(size_t i=0; i<iter->sensorData().stereoCameraModels().size(); ++i)
{
models.push_back(iter->sensorData().stereoCameraModels()[i].left());
}
}
if(!models.empty())
@@ -1793,25 +1796,25 @@ bool ExportCloudsDialog::getExportedClouds(
if(_ui->checkBox_fromDepth->isChecked())
{
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
std::vector<StereoCameraModel> stereoModels;
if(cachedSignatures.contains(iter->first))
{
const SensorData & data = cachedSignatures.find(iter->first)->sensorData();
models = data.cameraModels();
stereoModel = data.stereoCameraModel();
stereoModels = data.stereoCameraModels();
}
else if(_dbDriver)
{
_dbDriver->getCalibration(iter->first, models, stereoModel);
_dbDriver->getCalibration(iter->first, models, stereoModels);
}
if(models.size() && !models[0].localTransform().isNull())
{
iter->second *= models[0].localTransform();
}
else if(!stereoModel.localTransform().isNull())
else if(stereoModels.size() && !stereoModels[0].localTransform().isNull())
{
iter->second *= stereoModel.localTransform();
iter->second *= stereoModels[0].localTransform();
}
}
else
@@ -2260,16 +2263,16 @@ bool ExportCloudsDialog::getExportedClouds(
Eigen::Vector3f viewpoint(0.0f,0.0f,0.0f);
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
std::vector<StereoCameraModel> stereoModels;
if(cachedSignatures.contains(iter->first))
{
const SensorData & data = cachedSignatures.find(iter->first)->sensorData();
models = data.cameraModels();
stereoModel = data.stereoCameraModel();
stereoModels = data.stereoCameraModels();
}
else if(_dbDriver)
{
_dbDriver->getCalibration(iter->first, models, stereoModel);
_dbDriver->getCalibration(iter->first, models, stereoModels);
}
#ifdef RTABMAP_CPUTSDF
@@ -2340,11 +2343,11 @@ bool ExportCloudsDialog::getExportedClouds(
viewpoint[1] = models[0].localTransform().y();
viewpoint[2] = models[0].localTransform().z();
}
else if(!stereoModel.localTransform().isNull())
else if(stereoModels.size() && !stereoModels[0].localTransform().isNull())
{
viewpoint[0] = stereoModel.localTransform().x();
viewpoint[1] = stereoModel.localTransform().y();
viewpoint[2] = stereoModel.localTransform().z();
viewpoint[0] = stereoModels[0].localTransform().x();
viewpoint[1] = stereoModels[0].localTransform().y();
viewpoint[2] = stereoModels[0].localTransform().z();
}
}
@@ -2686,24 +2689,28 @@ bool ExportCloudsDialog::getExportedClouds(
for(std::map<int, Transform>::const_iterator iter=poses.lower_bound(0); iter!=poses.end(); ++iter)
{
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
std::vector<StereoCameraModel> stereoModels;
if(cachedSignatures.contains(iter->first))
{
const SensorData & data = cachedSignatures.find(iter->first)->sensorData();
models = data.cameraModels();
stereoModel = data.stereoCameraModel();
stereoModels = data.stereoCameraModels();
}
else if(_dbDriver)
{
_dbDriver->getCalibration(iter->first, models, stereoModel);
_dbDriver->getCalibration(iter->first, models, stereoModels);
}
if(stereoModel.isValidForProjection())
if(stereoModels.size())
{
models.clear();
models.push_back(stereoModel.left());
for(size_t i=0; i<stereoModels.size(); ++i)
{
models.push_back(stereoModels[i].left());
}
}
else if(models.size() == 0 || !models[0].isValidForProjection())
if(models.size() == 0 || !models[0].isValidForProjection())
{
models.clear();
}
@@ -3150,28 +3157,32 @@ bool ExportCloudsDialog::getExportedClouds(
if(validCameras.find(jter->first) != validCameras.end())
{
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
std::vector<StereoCameraModel> stereoModels;
bool cacheHasCompressedImage = false;
if(cachedSignatures.contains(jter->first))
{
const SensorData & data = cachedSignatures.find(jter->first)->sensorData();
models = data.cameraModels();
stereoModel = data.stereoCameraModel();
stereoModels = data.stereoCameraModels();
cacheHasCompressedImage = !data.imageCompressed().empty();
}
else if(_dbDriver)
{
_dbDriver->getCalibration(jter->first, models, stereoModel);
_dbDriver->getCalibration(jter->first, models, stereoModels);
}
bool stereo=false;
if(stereoModel.isValidForProjection())
if(stereoModels.size())
{
stereo = true;
models.clear();
models.push_back(stereoModel.left());
for(size_t i=0; i<stereoModels.size(); ++i)
{
models.push_back(stereoModels[i].left());
}
}
else if(models.size() == 0 || !models[0].isValidForProjection())
if(models.size() == 0 || !models[0].isValidForProjection())
{
models.clear();
}
@@ -3650,12 +3661,12 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
viewPoint[1] = data.cameraModels()[0].localTransform().y();
viewPoint[2] = data.cameraModels()[0].localTransform().z();
}
else if(!data.stereoCameraModel().localTransform().isNull())
else if(data.stereoCameraModels().size() && !data.stereoCameraModels()[0].localTransform().isNull())
{
localTransform = data.stereoCameraModel().localTransform();
viewPoint[0] = data.stereoCameraModel().localTransform().x();
viewPoint[1] = data.stereoCameraModel().localTransform().y();
viewPoint[2] = data.stereoCameraModel().localTransform().z();
localTransform = data.stereoCameraModels()[0].localTransform();
viewPoint[0] = data.stereoCameraModels()[0].localTransform().x();
viewPoint[1] = data.stereoCameraModels()[0].localTransform().y();
viewPoint[2] = data.stereoCameraModels()[0].localTransform().z();
}
if(_ui->spinBox_normalKSearch->value()>0 || _ui->doubleSpinBox_normalRadiusSearch->value()>0.0)
@@ -3766,16 +3777,16 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
// view point
Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
std::vector<CameraModel> models;
StereoCameraModel stereoModel;
std::vector<StereoCameraModel> stereoModels;
if(cachedSignatures.contains(iter->first))
{
const Signature & s = cachedSignatures.find(iter->first).value();
models = s.sensorData().cameraModels();
stereoModel = s.sensorData().stereoCameraModel();
stereoModels = s.sensorData().stereoCameraModels();
}
else if(_dbDriver)
{
_dbDriver->getCalibration(iter->first, models, stereoModel);
_dbDriver->getCalibration(iter->first, models, stereoModels);
}
if(models.size() && !models[0].localTransform().isNull())
@@ -3785,12 +3796,12 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
viewPoint[1] = models[0].localTransform().y();
viewPoint[2] = models[0].localTransform().z();
}
else if(!stereoModel.localTransform().isNull())
else if(stereoModels.size() && !stereoModels[0].localTransform().isNull())
{
localTransform = stereoModel.localTransform();
viewPoint[0] = stereoModel.localTransform().x();
viewPoint[1] = stereoModel.localTransform().y();
viewPoint[2] = stereoModel.localTransform().z();
localTransform = stereoModels[0].localTransform();
viewPoint[0] = stereoModels[0].localTransform().x();
viewPoint[1] = stereoModels[0].localTransform().y();
viewPoint[2] = stereoModels[0].localTransform().z();
}
else
{
@@ -3863,7 +3874,7 @@ std::map<int, std::pair<pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr, pcl::Indic
util3d::transformPointCloud(cloud, iter->second),
indices,
"z",
min!=0.0f&&min<max?min:std::numeric_limits<int>::min(),
min!=0.0f&&(min<max || max==0.0f)?min:std::numeric_limits<int>::min(),
max!=0.0f?max:std::numeric_limits<int>::max());
}
if(!indices->empty() &&
@@ -4494,9 +4505,12 @@ void ExportCloudsDialog::saveTextureMeshes(
{
models = iter->sensorData().cameraModels();
}
else if(iter->sensorData().stereoCameraModel().isValidForProjection())
else if(iter->sensorData().stereoCameraModels().size())
{
models.push_back(iter->sensorData().stereoCameraModel().left());
for(size_t i=0; i<iter->sensorData().stereoCameraModels().size(); ++i)
{
models.push_back(iter->sensorData().stereoCameraModels()[i].left());
}
}
if(!models.empty())
@@ -4693,8 +4707,15 @@ void ExportCloudsDialog::saveTextureMeshes(
SensorData data;
_dbDriver->getNodeData(textureId, data, true, false, false, false);
data.uncompressDataConst(&image, 0);
StereoCameraModel stereoModel;
_dbDriver->getCalibration(textureId, cameraModels, stereoModel);
std::vector<StereoCameraModel> stereoModels;
_dbDriver->getCalibration(textureId, cameraModels, stereoModels);
if(cameraModels.empty())
{
for(size_t i=0; i<stereoModels.size(); ++i)
{
cameraModels.push_back(stereoModels[i].left());
}
}
}
previousImage = image;
@@ -4878,8 +4899,15 @@ void ExportCloudsDialog::saveTextureMeshes(
SensorData data;
_dbDriver->getNodeData(textureId, data, true, false, false, false);
data.uncompressDataConst(&image, 0);
StereoCameraModel stereoModel;
_dbDriver->getCalibration(textureId, cameraModels, stereoModel);
std::vector<StereoCameraModel> stereoModels;
_dbDriver->getCalibration(textureId, cameraModels, stereoModels);
if(cameraModels.empty())
{
for(size_t i=0; i<stereoModels.size(); ++i)
{
cameraModels.push_back(stereoModels[i].left());
}
}
}
previousImage = image;

185
guilib/src/MainWindow.cpp
View File

@@ -949,7 +949,7 @@ bool MainWindow::handleEvent(UEvent* anEvent)
// we receive too many odometry events! just send without data
SensorData data(cv::Mat(), odomEvent->data().id(), odomEvent->data().stamp());
data.setCameraModels(odomEvent->data().cameraModels());
data.setStereoCameraModel(odomEvent->data().stereoCameraModel());
data.setStereoCameraModels(odomEvent->data().stereoCameraModels());
data.setGroundTruth(odomEvent->data().groundTruth());
OdometryEvent tmp(data, odomEvent->pose(), odomEvent->info().copyWithoutData());
Q_EMIT odometryReceived(tmp, true);
@@ -1113,34 +1113,51 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
}
rectifiedData.setRGBDImage(rectifiedImages, data->depthOrRightRaw(), data->cameraModels());
}
else if(!data->rightRaw().empty())
else if(!data->rightRaw().empty() && data->stereoCameraModels().size())
{
if(data->stereoCameraModel().isValidForRectification())
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();
bool initRectMaps = _rectCameraModelsOdom.empty() || _rectCameraModelsOdom.size()!=data->stereoCameraModels().size()*2;
if(initRectMaps)
{
bool initRectMaps = _rectCameraModelsOdom.size()!=2;
if(initRectMaps)
_rectCameraModelsOdom.resize(data->stereoCameraModels().size()*2);
}
for(unsigned int i=0; i<data->stereoCameraModels().size(); ++i)
{
if(data->stereoCameraModels()[i].isValidForRectification())
{
_rectCameraModelsOdom.resize(2);
_rectCameraModelsOdom[0] = data->stereoCameraModel().left();
_rectCameraModelsOdom[1] = data->stereoCameraModel().right();
if(!_rectCameraModelsOdom[0].isRectificationMapInitialized())
if(initRectMaps)
{
UWARN("Initializing rectification maps for stereo camera (only done for the first image received)...");
_rectCameraModelsOdom[0].initRectificationMap();
_rectCameraModelsOdom[1].initRectificationMap();
UWARN("Initializing rectification maps for stereo camera (only done for the first image received)... done!");
_rectCameraModelsOdom[i*2] = data->stereoCameraModels()[i].left();
_rectCameraModelsOdom[i*2+1] = data->stereoCameraModels()[i].right();
if(!_rectCameraModelsOdom[i*2].isRectificationMapInitialized())
{
UWARN("Initializing rectification maps for stereo camera %d (only done for the first image received)...", i);
_rectCameraModelsOdom[i*2].initRectificationMap();
_rectCameraModelsOdom[i*2+1].initRectificationMap();
UWARN("Initializing rectification maps for stereo camera %d (only done for the first image received)... done!", i);
}
}
UASSERT(_rectCameraModelsOdom[i*2].imageWidth() == data->stereoCameraModels()[i].left().imageWidth() &&
_rectCameraModelsOdom[i*2].imageHeight() == data->stereoCameraModels()[i].left().imageHeight() &&
_rectCameraModelsOdom[i*2+1].imageWidth() == data->stereoCameraModels()[i].right().imageWidth() &&
_rectCameraModelsOdom[i*2+1].imageHeight() == data->stereoCameraModels()[i].right().imageHeight());
cv::Mat rectifiedLeftImage = _rectCameraModelsOdom[i*2].rectifyImage(cv::Mat(data->imageRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data->imageRaw().rows)));
cv::Mat rectifiedRightImage = _rectCameraModelsOdom[i*2+1].rectifyImage(cv::Mat(data->rightRaw(), cv::Rect(subImageWidth*i, 0, subImageWidth, data->rightRaw().rows)));
rectifiedLeftImage.copyTo(cv::Mat(rectifiedLeftImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data->imageRaw().rows)));
rectifiedRightImage.copyTo(cv::Mat(rectifiedRightImages, cv::Rect(subImageWidth*i, 0, subImageWidth, data->rightRaw().rows)));
}
else
{
UWARN("Stereo camera %d of data %d is not valid for rectification (%dx%d).",
i, data->id(),
data->stereoCameraModels()[i].left().imageWidth(),
data->stereoCameraModels()[i].right().imageHeight());
}
UASSERT(_rectCameraModelsOdom[0].imageWidth() == data->stereoCameraModel().left().imageWidth() &&
_rectCameraModelsOdom[1].imageHeight() == data->stereoCameraModel().right().imageHeight());
cv::Mat rectifiedLeft = _rectCameraModelsOdom[0].rectifyImage(data->imageRaw());
cv::Mat rectifiedRight = _rectCameraModelsOdom[1].rectifyImage(data->rightRaw());
rectifiedData.setStereoImage(rectifiedLeft, rectifiedRight, data->stereoCameraModel());
}
else
{
UWARN("Stereo camera model of data %d is not valid for rectification.", data->id());
}
rectifiedData.setStereoImage(rectifiedLeftImages, rectifiedRightImages, data->stereoCameraModels());
}
UDEBUG("Time rectification: %fs", time.ticks());
data = &rectifiedData;
@@ -1159,7 +1176,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
// 3d cloud
if(!data->imageRaw().empty() &&
!data->depthOrRightRaw().empty() &&
(data->cameraModels().size() || data->stereoCameraModel().isValidForProjection()) &&
(data->cameraModels().size() || data->stereoCameraModels().size()) &&
_preferencesDialog->isCloudsShown(1))
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud;
@@ -1190,11 +1207,11 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
viewpoint[1] = data->cameraModels()[0].localTransform().y();
viewpoint[2] = data->cameraModels()[0].localTransform().z();
}
else if(!data->stereoCameraModel().localTransform().isNull())
else if(data->stereoCameraModels().size() && !data->stereoCameraModels()[0].localTransform().isNull())
{
viewpoint[0] = data->stereoCameraModel().localTransform().x();
viewpoint[1] = data->stereoCameraModel().localTransform().y();
viewpoint[2] = data->stereoCameraModel().localTransform().z();
viewpoint[0] = data->stereoCameraModels()[0].localTransform().x();
viewpoint[1] = data->stereoCameraModels()[0].localTransform().y();
viewpoint[2] = data->stereoCameraModels()[0].localTransform().z();
}
std::vector<pcl::Vertices> polygons = util3d::organizedFastMesh(
output,
@@ -1427,6 +1444,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
if(splitted.size() == 2)
{
int id = std::atoi(splitted.back().c_str());
id -= id%10;
if(splitted.front().compare("f_odom_") == 0 &&
odom.info().localBundlePoses.find(id) == odom.info().localBundlePoses.end())
{
@@ -1437,19 +1455,27 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
for(std::map<int, Transform>::const_iterator iter=odom.info().localBundlePoses.begin();iter!=odom.info().localBundlePoses.end(); ++iter)
{
std::string frustumId = uFormat("f_odom_%d", iter->first);
std::string frustumId = uFormat("f_odom_%d", iter->first*10);
if(_cloudViewer->getAddedFrustums().contains(frustumId))
{
_cloudViewer->updateFrustumPose(frustumId, _odometryCorrection*iter->second);
for(size_t i=0; i<10; ++i)
{
std::string subFrustumId = uFormat("f_odom_%d", iter->first*10+i);
_cloudViewer->updateFrustumPose(subFrustumId, _odometryCorrection*iter->second);
}
}
else if(odom.info().localBundleModels.find(iter->first) != odom.info().localBundleModels.end())
{
const CameraModel & model = odom.info().localBundleModels.at(iter->first);
Transform t = model.localTransform();
if(!t.isNull())
const std::vector<CameraModel> & models = odom.info().localBundleModels.at(iter->first);
for(size_t i=0; i<models.size(); ++i)
{
QColor color = Qt::yellow;
_cloudViewer->addOrUpdateFrustum(frustumId, _odometryCorrection*iter->second, t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
Transform t = models[i].localTransform();
if(!t.isNull())
{
QColor color = Qt::yellow;
std::string subFrustumId = uFormat("f_odom_%d", iter->first*10+i);
_cloudViewer->addOrUpdateFrustum(subFrustumId, _odometryCorrection*iter->second, t, _cloudViewer->getFrustumScale(), color, models[i].fovX(), models[i].fovY());
}
}
}
}
@@ -1519,9 +1545,9 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
{
_cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), data->cameraModels());
}
else if(data->stereoCameraModel().isValidForProjection())
else if(data->stereoCameraModels().size() && data->stereoCameraModels()[0].isValidForProjection())
{
_cloudViewer->updateCameraFrustum(_odometryCorrection*odom.pose(), data->stereoCameraModel());
_cloudViewer->updateCameraFrustums(_odometryCorrection*odom.pose(), data->stereoCameraModels());
}
else if(!data->laserScanRaw().isEmpty() ||
!data->laserScanCompressed().isEmpty())
@@ -2198,9 +2224,13 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
sceneRect.setHeight(sceneRect.height()+signature.sensorData().cameraModels()[i].imageHeight());
}
}
else if(signature.sensorData().stereoCameraModel().isValidForProjection())
else if(signature.sensorData().stereoCameraModels().size())
{
sceneRect.setRect(0,0,signature.sensorData().stereoCameraModel().left().imageWidth(), signature.sensorData().stereoCameraModel().left().imageHeight());
for(unsigned int i=0; i<signature.sensorData().cameraModels().size(); ++i)
{
sceneRect.setWidth(sceneRect.width()+signature.sensorData().stereoCameraModels()[i].left().imageWidth());
sceneRect.setHeight(sceneRect.height()+signature.sensorData().stereoCameraModels()[i].left().imageHeight());
}
}
if(sceneRect.isValid())
{
@@ -2366,9 +2396,9 @@ void MainWindow::processStats(const rtabmap::Statistics & stat)
{
_cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().cameraModels());
}
else if(stat.getLastSignatureData().sensorData().stereoCameraModel().isValidForProjection())
else if(stat.getLastSignatureData().sensorData().stereoCameraModels().size() && stat.getLastSignatureData().sensorData().stereoCameraModels()[0].isValidForProjection())
{
_cloudViewer->updateCameraFrustum(poses.rbegin()->second, stat.getLastSignatureData().sensorData().stereoCameraModel());
_cloudViewer->updateCameraFrustums(poses.rbegin()->second, stat.getLastSignatureData().sensorData().stereoCameraModels());
}
else if(!stat.getLastSignatureData().sensorData().laserScanRaw().isEmpty() ||
!stat.getLastSignatureData().sensorData().laserScanCompressed().isEmpty())
@@ -3015,9 +3045,9 @@ void MainWindow::updateMapCloud(
{
const Signature & s = _cachedSignatures.value(iter->first);
// Supporting only one frustum per node
if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModel().isValidForProjection())
if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModels().size()==1)
{
const CameraModel & model = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().left():s.sensorData().cameraModels()[0];
const CameraModel & model = s.sensorData().stereoCameraModels().size()?s.sensorData().stereoCameraModels()[0].left():s.sensorData().cameraModels()[0];
Transform t = model.localTransform();
if(!t.isNull())
{
@@ -3441,11 +3471,11 @@ std::pair<pcl::PointCloud<pcl::PointXYZRGB>::Ptr, pcl::IndicesPtr> MainWindow::c
viewPoint[1] = data.cameraModels()[0].localTransform().y();
viewPoint[2] = data.cameraModels()[0].localTransform().z();
}
else if(!data.stereoCameraModel().localTransform().isNull())
else if(data.stereoCameraModels().size() && !data.stereoCameraModels()[0].localTransform().isNull())
{
viewPoint[0] = data.stereoCameraModel().localTransform().x();
viewPoint[1] = data.stereoCameraModel().localTransform().y();
viewPoint[2] = data.stereoCameraModel().localTransform().z();
viewPoint[0] = data.stereoCameraModels()[0].localTransform().x();
viewPoint[1] = data.stereoCameraModels()[0].localTransform().y();
viewPoint[2] = data.stereoCameraModels()[0].localTransform().z();
}
// filtering pipeline
@@ -4077,13 +4107,15 @@ void MainWindow::createAndAddFeaturesToMap(int nodeId, const Transform & pose, i
if(!iter->getWords3().empty() && !iter->getWordsKpts().empty())
{
Transform invLocalTransform = Transform::getIdentity();
if(iter.value().sensorData().cameraModels().size() == 1 && iter.value().sensorData().cameraModels().at(0).isValidForProjection())
if(iter.value().sensorData().cameraModels().size() == 1 &&
iter.value().sensorData().cameraModels().at(0).isValidForProjection())
{
invLocalTransform = iter.value().sensorData().cameraModels()[0].localTransform().inverse();
}
else if(iter.value().sensorData().stereoCameraModel().isValidForProjection())
else if(iter.value().sensorData().stereoCameraModels().size() == 1 &&
iter.value().sensorData().stereoCameraModels()[0].isValidForProjection())
{
invLocalTransform = iter.value().sensorData().stereoCameraModel().left().localTransform().inverse();
invLocalTransform = iter.value().sensorData().stereoCameraModels()[0].left().localTransform().inverse();
}
for(std::multimap<int, int>::const_iterator jter=iter->getWords().begin(); jter!=iter->getWords().end(); ++jter)
@@ -4883,9 +4915,10 @@ void MainWindow::drawLandmarks(cv::Mat & image, const Signature & signature)
{
model = signature.sensorData().cameraModels()[0];
}
else if(signature.sensorData().stereoCameraModel().isValidForProjection())
else if(!signature.sensorData().stereoCameraModels().empty() &&
signature.sensorData().stereoCameraModels()[0].isValidForProjection())
{
model = signature.sensorData().stereoCameraModel().left();
model = signature.sensorData().stereoCameraModels()[0].left();
}
if(model.isValidForProjection())
{
@@ -5941,16 +5974,18 @@ void MainWindow::exportPoses(int format)
Transform localTransform;
if(cameraFrame)
{
if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 &&
!_cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform().isNull()))
if(_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 &&
!_cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform().isNull())
{
localTransform = _cachedSignatures[iter->first].sensorData().cameraModels().at(0).localTransform();
}
else if(!_cachedSignatures[iter->first].sensorData().stereoCameraModel().localTransform().isNull())
else if(_cachedSignatures[iter->first].sensorData().stereoCameraModels().size() == 1 &&
!_cachedSignatures[iter->first].sensorData().stereoCameraModels()[0].localTransform().isNull())
{
localTransform = _cachedSignatures[iter->first].sensorData().stereoCameraModel().localTransform();
localTransform = _cachedSignatures[iter->first].sensorData().stereoCameraModels()[0].localTransform();
}
else if(_cachedSignatures[iter->first].sensorData().cameraModels().size()>1)
else if(_cachedSignatures[iter->first].sensorData().cameraModels().size()>1 ||
_cachedSignatures[iter->first].sensorData().stereoCameraModels().size()>1)
{
UWARN("Multi-camera is not supported (node %d)", iter->first);
}
@@ -7707,26 +7742,30 @@ void MainWindow::exportImages()
QDir dir;
dir.mkdir(QString("%1/left").arg(path));
dir.mkdir(QString("%1/right").arg(path));
if(data.stereoCameraModel().isValidForProjection())
if(data.stereoCameraModels().size() > 1)
{
UERROR("Only one stereo camera calibration can be saved at this time (%d detected)", (int)data.stereoCameraModels().size());
}
else if(data.stereoCameraModels().size() == 1 && data.stereoCameraModels().front().isValidForProjection())
{
std::string cameraName = "calibration";
StereoCameraModel model(
cameraName,
data.imageRaw().size(),
data.stereoCameraModel().left().K(),
data.stereoCameraModel().left().D(),
data.stereoCameraModel().left().R(),
data.stereoCameraModel().left().P(),
data.stereoCameraModels()[0].left().K(),
data.stereoCameraModels()[0].left().D(),
data.stereoCameraModels()[0].left().R(),
data.stereoCameraModels()[0].left().P(),
data.rightRaw().size(),
data.stereoCameraModel().right().K(),
data.stereoCameraModel().right().D(),
data.stereoCameraModel().right().R(),
data.stereoCameraModel().right().P(),
data.stereoCameraModel().R(),
data.stereoCameraModel().T(),
data.stereoCameraModel().E(),
data.stereoCameraModel().F(),
data.stereoCameraModel().left().localTransform());
data.stereoCameraModels()[0].right().K(),
data.stereoCameraModels()[0].right().D(),
data.stereoCameraModels()[0].right().R(),
data.stereoCameraModels()[0].right().P(),
data.stereoCameraModels()[0].R(),
data.stereoCameraModels()[0].T(),
data.stereoCameraModels()[0].E(),
data.stereoCameraModels()[0].F(),
data.stereoCameraModels()[0].left().localTransform());
if(model.save(path.toStdString()))
{
calibrationSaved = true;
@@ -7880,8 +7919,10 @@ void MainWindow::exportBundlerFormat()
{
UWARN("Missing image in cache for node %d", iter->first);
}
else if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 && _cachedSignatures[iter->first].sensorData().cameraModels().at(0).isValidForProjection()) ||
_cachedSignatures[iter->first].sensorData().stereoCameraModel().isValidForProjection())
else if((_cachedSignatures[iter->first].sensorData().cameraModels().size() == 1 &&
_cachedSignatures[iter->first].sensorData().cameraModels().at(0).isValidForProjection()) ||
(_cachedSignatures[iter->first].sensorData().stereoCameraModels().size() == 1 &&
_cachedSignatures[iter->first].sensorData().stereoCameraModels()[0].isValidForProjection()))
{
poses.insert(*iter);
}

6
guilib/src/OdometryViewer.cpp
View File

@@ -216,7 +216,7 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
if(cloudShown_->isChecked() &&
!odom.data().imageRaw().empty() &&
!odom.data().depthOrRightRaw().empty() &&
(odom.data().stereoCameraModel().isValidForProjection() || odom.data().cameraModels().size()))
(odom.data().stereoCameraModels().size() || odom.data().cameraModels().size()))
{
UDEBUG("New pose = %s, quality=%d", odom.pose().prettyPrint().c_str(), quality);
@@ -303,9 +303,9 @@ void OdometryViewer::processData(const rtabmap::OdometryEvent & odom)
{
cloudView_->updateCameraFrustums(odom.pose(), odom.data().cameraModels());
}
else if(!odom.data().stereoCameraModel().localTransform().isNull())
else if(odom.data().stereoCameraModels().size() && !odom.data().stereoCameraModels()[0].localTransform().isNull())
{
cloudView_->updateCameraFrustum(odom.pose(), odom.data().stereoCameraModel());
cloudView_->updateCameraFrustums(odom.pose(), odom.data().stereoCameraModels());
}
}

41
guilib/src/PreferencesDialog.cpp
View File

@@ -795,6 +795,7 @@ PreferencesDialog::PreferencesDialog(QWidget * parent) :
connect(_ui->comboBox_depthai_resolution, SIGNAL(currentIndexChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_depthai_depth, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_depthai_confidence, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkBox_depthai_imu_firmware_update, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->checkbox_rgbd_colorOnly, SIGNAL(stateChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
connect(_ui->spinBox_source_imageDecimation, SIGNAL(valueChanged(int)), this, SLOT(makeObsoleteSourcePanel()));
@@ -1919,8 +1920,8 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->source_checkBox_ignoreOdometry->setChecked(false);
_ui->source_checkBox_ignoreGoalDelay->setChecked(true);
_ui->source_checkBox_ignoreGoals->setChecked(true);
_ui->source_checkBox_ignoreLandmarks->setChecked(false);
_ui->source_checkBox_ignoreFeatures->setChecked(false);
_ui->source_checkBox_ignoreLandmarks->setChecked(true);
_ui->source_checkBox_ignoreFeatures->setChecked(true);
_ui->source_spinBox_databaseStartId->setValue(0);
_ui->source_spinBox_databaseStopId->setValue(0);
_ui->source_spinBox_database_cameraIndex->setValue(-1);
@@ -2039,6 +2040,7 @@ void PreferencesDialog::resetSettings(QGroupBox * groupBox)
_ui->comboBox_depthai_resolution->setCurrentIndex(1);
_ui->checkBox_depthai_depth->setChecked(false);
_ui->spinBox_depthai_confidence->setValue(200);
_ui->checkBox_depthai_imu_firmware_update->setChecked(false);
_ui->checkBox_cameraImages_configForEachFrame->setChecked(false);
_ui->checkBox_cameraImages_timestamps->setChecked(false);
@@ -2519,6 +2521,7 @@ void PreferencesDialog::readCameraSettings(const QString & filePath)
_ui->comboBox_depthai_resolution->setCurrentIndex(settings.value("resolution", _ui->comboBox_depthai_resolution->currentIndex()).toInt());
_ui->checkBox_depthai_depth->setChecked(settings.value("depth", _ui->checkBox_depthai_depth->isChecked()).toBool());
_ui->spinBox_depthai_confidence->setValue(settings.value("confidence", _ui->spinBox_depthai_confidence->value()).toInt());
_ui->checkBox_depthai_imu_firmware_update->setChecked(settings.value("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked()).toBool());
settings.endGroup(); // DepthAI
settings.beginGroup("Images");
@@ -3041,6 +3044,7 @@ void PreferencesDialog::writeCameraSettings(const QString & filePath) const
settings.setValue("resolution", _ui->comboBox_depthai_resolution->currentIndex());
settings.setValue("depth", _ui->checkBox_depthai_depth->isChecked());
settings.setValue("confidence", _ui->spinBox_depthai_confidence->value());
settings.setValue("imu_firmware_update", _ui->checkBox_depthai_imu_firmware_update->isChecked());
settings.endGroup(); // DepthAI
settings.beginGroup("Images");
@@ -5886,9 +5890,6 @@ Camera * PreferencesDialog::createCamera(bool useRawImages, bool useColor)
this->getSourceDriver(),
_ui->lineEdit_sourceDevice->text(),
_ui->lineEdit_calibrationFile->text(),
(this->getSourceDriver()>=kSrcStereo &&
this->getSourceDriver()<kSrcRGB &&
_ui->checkBox_stereo_rectify->isEnabled() && !_ui->checkBox_stereo_rectify->isChecked()) ||
useRawImages,
useColor,
false,
@@ -6049,7 +6050,7 @@ Camera * PreferencesDialog::createCamera(
((CameraRealSense2*)camera)->publishInterIMU(_ui->checkbox_publishInterIMU->isChecked());
if(driver == kSrcStereoRealSense2)
{
((CameraRealSense2*)camera)->setImagesRectified(!useRawImages);
((CameraRealSense2*)camera)->setImagesRectified((_ui->checkBox_stereo_rectify->isEnabled() && _ui->checkBox_stereo_rectify->isChecked()) && !useRawImages);
((CameraRealSense2*)camera)->setOdomProvided(_ui->comboBox_odom_sensor->currentIndex() == 1 || odomOnly, odomOnly, odomSensorExtrinsicsCalib);
}
else
@@ -6148,7 +6149,7 @@ Camera * PreferencesDialog::createCamera(
camera = new CameraStereoImages(
_ui->lineEdit_cameraStereoImages_path_left->text().append(QDir::separator()).toStdString(),
_ui->lineEdit_cameraStereoImages_path_right->text().append(QDir::separator()).toStdString(),
!useRawImages,
(_ui->checkBox_stereo_rectify->isEnabled() && _ui->checkBox_stereo_rectify->isChecked()) && !useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
((CameraStereoImages*)camera)->setStartIndex(_ui->spinBox_cameraStereoImages_startIndex->value());
@@ -6175,7 +6176,7 @@ Camera * PreferencesDialog::createCamera(
camera = new CameraStereoVideo(
device.isEmpty() ? 0 : atoi(device.toStdString().c_str()),
_ui->spinBox_stereo_right_device->value(),
!useRawImages,
(_ui->checkBox_stereo_rectify->isEnabled() && _ui->checkBox_stereo_rectify->isChecked()) && !useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -6183,7 +6184,7 @@ Camera * PreferencesDialog::createCamera(
{
camera = new CameraStereoVideo(
device.isEmpty() ? 0 : atoi(device.toStdString().c_str()),
!useRawImages,
(_ui->checkBox_stereo_rectify->isEnabled() && _ui->checkBox_stereo_rectify->isChecked()) && !useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -6197,7 +6198,7 @@ Camera * PreferencesDialog::createCamera(
camera = new CameraStereoVideo(
_ui->lineEdit_cameraStereoVideo_path->text().toStdString(),
_ui->lineEdit_cameraStereoVideo_path_2->text().toStdString(),
!useRawImages,
(_ui->checkBox_stereo_rectify->isEnabled() && _ui->checkBox_stereo_rectify->isChecked()) && !useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -6206,7 +6207,7 @@ Camera * PreferencesDialog::createCamera(
// side-by-side video
camera = new CameraStereoVideo(
_ui->lineEdit_cameraStereoVideo_path->text().toStdString(),
!useRawImages,
(_ui->checkBox_stereo_rectify->isEnabled() && _ui->checkBox_stereo_rectify->isChecked()) && !useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -6217,7 +6218,7 @@ Camera * PreferencesDialog::createCamera(
camera = new CameraStereoTara(
device.isEmpty()?0:atoi(device.toStdString().c_str()),
!useRawImages,
(_ui->checkBox_stereo_rectify->isEnabled() && _ui->checkBox_stereo_rectify->isChecked()) && !useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
@@ -6275,12 +6276,13 @@ Camera * PreferencesDialog::createCamera(
this->getGeneralInputRate(),
this->getSourceLocalTransform());
((CameraDepthAI*)camera)->setOutputDepth(_ui->checkBox_depthai_depth->isChecked(), _ui->spinBox_depthai_confidence->value());
((CameraDepthAI*)camera)->setIMUFirmwareUpdate(_ui->checkBox_depthai_imu_firmware_update->isChecked());
}
else if(driver == kSrcUsbDevice)
{
camera = new CameraVideo(
device.isEmpty()?0:atoi(device.toStdString().c_str()),
!useRawImages,
(_ui->checkBox_rgb_rectify->isEnabled() && _ui->checkBox_rgb_rectify->isChecked()) && !useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
((CameraVideo*)camera)->setResolution(_ui->spinBox_usbcam_streamWidth->value(), _ui->spinBox_usbcam_streamHeight->value());
@@ -6289,7 +6291,7 @@ Camera * PreferencesDialog::createCamera(
{
camera = new CameraVideo(
_ui->source_video_lineEdit_path->text().toStdString(),
!useRawImages,
(_ui->checkBox_rgb_rectify->isEnabled() && _ui->checkBox_rgb_rectify->isChecked()) && !useRawImages,
this->getGeneralInputRate(),
this->getSourceLocalTransform());
}
@@ -6302,7 +6304,7 @@ Camera * PreferencesDialog::createCamera(
((CameraImages*)camera)->setStartIndex(_ui->source_images_spinBox_startPos->value());
((CameraImages*)camera)->setMaxFrames(_ui->source_images_spinBox_maxFrames->value());
((CameraImages*)camera)->setImagesRectified(!useRawImages);
((CameraImages*)camera)->setImagesRectified((_ui->checkBox_rgb_rectify->isEnabled() && _ui->checkBox_rgb_rectify->isChecked()) && !useRawImages);
((CameraImages*)camera)->setBayerMode(_ui->comboBox_cameraImages_bayerMode->currentIndex()-1);
((CameraImages*)camera)->setOdometryPath(
@@ -6365,6 +6367,7 @@ Camera * PreferencesDialog::createCamera(
}
}
UDEBUG("useRawImages=%d dir=%s", useRawImages?1:0, dir.toStdString().c_str());
// don't set calibration folder if we want raw images
if(!camera->init(useRawImages?"":dir.toStdString(), name.toStdString()))
{
@@ -7113,8 +7116,8 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics()
if(odomSensorData.cameraModels().size() == 1) {
odomSensorModel = odomSensorData.cameraModels()[0];
}
else {
odomSensorModel = odomSensorData.stereoCameraModel().left();
else if(odomSensorData.stereoCameraModels().size() == 1) {
odomSensorModel = odomSensorData.stereoCameraModels()[0].left();
}
delete camera;
@@ -7138,8 +7141,8 @@ void PreferencesDialog::calibrateOdomSensorExtrinsics()
if(camData.cameraModels().size() == 1) {
cameraModel = camData.cameraModels()[0];
}
else {
cameraModel = camData.stereoCameraModel().left();
else if(camData.stereoCameraModels().size() == 1) {
cameraModel = camData.stereoCameraModels()[0].left();
}
delete camera;

76
guilib/src/ui/preferencesDialog.ui
View File

@@ -63,9 +63,9 @@
<property name="geometry">
<rect>
<x>0</x>
<y>-1042</y>
<y>-454</y>
<width>756</width>
<height>3640</height>
<height>3657</height>
</rect>
</property>
<layout class="QVBoxLayout" name="verticalLayout_16">
@@ -3109,7 +3109,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_src">
<property name="currentIndex">
<number>3</number>
<number>1</number>
</property>
<widget class="QWidget" name="page_41">
<layout class="QVBoxLayout" name="verticalLayout_64">
@@ -4892,7 +4892,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<item>
<widget class="QStackedWidget" name="stackedWidget_stereo">
<property name="currentIndex">
<number>3</number>
<number>10</number>
</property>
<widget class="QWidget" name="page_49">
<layout class="QVBoxLayout" name="verticalLayout_91"/>
@@ -5710,16 +5710,10 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
<string>DepthAI</string>
</property>
<layout class="QGridLayout" name="gridLayout_120" columnstretch="0,1">
<item row="0" column="1">
<widget class="QLabel" name="label_626">
<item row="1" column="0">
<widget class="QCheckBox" name="checkBox_depthai_depth">
<property name="text">
<string>Resolution.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
<string/>
</property>
</widget>
</item>
@@ -5736,13 +5730,6 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="1" column="0">
<widget class="QCheckBox" name="checkBox_depthai_depth">
<property name="text">
<string/>
</property>
</widget>
</item>
<item row="0" column="0">
<widget class="QComboBox" name="comboBox_depthai_resolution">
<property name="currentIndex">
@@ -5768,6 +5755,32 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</item>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="spinBox_depthai_confidence">
<property name="minimum">
<number>0</number>
</property>
<property name="maximum">
<number>255</number>
</property>
<property name="value">
<number>200</number>
</property>
</widget>
</item>
<item row="0" column="1">
<widget class="QLabel" name="label_626">
<property name="text">
<string>Resolution.</string>
</property>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="2" column="1">
<widget class="QLabel" name="label_627">
<property name="text">
@@ -5781,16 +5794,23 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
</property>
</widget>
</item>
<item row="2" column="0">
<widget class="QSpinBox" name="spinBox_depthai_confidence">
<property name="minimum">
<number>0</number>
<item row="3" column="1">
<widget class="QLabel" name="label_646">
<property name="text">
<string>IMU firmware update</string>
</property>
<property name="maximum">
<number>255</number>
<property name="wordWrap">
<bool>true</bool>
</property>
<property name="value">
<number>200</number>
<property name="textInteractionFlags">
<set>Qt::LinksAccessibleByMouse|Qt::TextSelectableByMouse</set>
</property>
</widget>
</item>
<item row="3" column="0">
<widget class="QCheckBox" name="checkBox_depthai_imu_firmware_update">
<property name="text">
<string/>
</property>
</widget>
</item>