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

CloudViewer: adjust frustum fov based on camera model (https://github.com/introlab/rtabmap_ros/issues/419)

Browse Source
This commit is contained in:
matlabbe
2020-05-24 11:13:13 -04:00
parent eb9999d7b1
commit 6acfc62196
6 changed files with 33 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
corelib/include/rtabmap/core/CameraModel.h
+3
View File
@@ -113,6 +113,9 @@ public:
int imageWidth() const {return imageSize_.width;} int imageWidth() const {return imageSize_.width;}
int imageHeight() const {return imageSize_.height;} int imageHeight() const {return imageSize_.height;}
double fovX() const {return imageSize_.width>0 && fx()>0?2.0*atan(imageSize_.width/(fx()*2.0)):0.0;}
double fovY() const {return imageSize_.height>0 && fy()>0?2.0*atan(imageSize_.height/(fy()*2.0)):0.0;}
bool load(const std::string & directory, const std::string & cameraName); bool load(const std::string & directory, const std::string & cameraName);
bool save(const std::string & directory) const; bool save(const std::string & directory) const;
std::vector<unsigned char> serialize() const; std::vector<unsigned char> serialize() const;
corelib/src/Parameters.cpp
+6
View File
@@ -733,6 +733,12 @@ ParametersMap Parameters::parseArguments(int argc, char * argv[], bool onlyParam
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl; std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else #else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl; std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif
str = "With MYNT EYE S:";
#ifdef RTABMAP_MYNTEYE
std::cout << str << std::setw(spacing - str.size()) << "true" << std::endl;
#else
std::cout << str << std::setw(spacing - str.size()) << "false" << std::endl;
#endif #endif
str = "With libpointmatcher:"; str = "With libpointmatcher:";
#ifdef RTABMAP_POINTMATCHER #ifdef RTABMAP_POINTMATCHER
guilib/include/rtabmap/gui/CloudViewer.h
+4 -2
View File
@@ -256,10 +256,12 @@ public:
void addOrUpdateFrustum( void addOrUpdateFrustum(
const std::string & id, const std::string & id,
const Transform & transform, const Transform & pose,
const Transform & localTransform, const Transform & localTransform,
double scale, double scale,
const QColor & color = QColor()); const QColor & color = QColor(),
float fovX=1.1,
float fovY=0.85);
bool updateFrustumPose( bool updateFrustumPose(
const std::string & id, const std::string & id,
const Transform & pose); const Transform & pose);
guilib/src/CloudViewer.cpp
+14 -12
View File
@@ -1831,18 +1831,20 @@ static const float frustum_vertices[] = {
0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f,
1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f,
1.0f, -1.0f, 1.0f, 1.0f, -1.0f, 1.0f,
1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f,
1.0f, 1.0f, -1.0f}; -1.0f, 1.0f, 1.0f};
static const int frustum_indices[] = { static const int frustum_indices[] = {
1, 2, 3, 4, 1, 0, 2, 0, 3, 0, 4}; 1, 2, 3, 4, 1, 0, 2, 0, 3, 0, 4};
void CloudViewer::addOrUpdateFrustum( void CloudViewer::addOrUpdateFrustum(
const std::string & id, const std::string & id,
const Transform & transform, const Transform & pose,
const Transform & localTransform, const Transform & localTransform,
double scale, double scale,
const QColor & color) const QColor & color,
float fovX,
float fovY)
{ {
if(id.empty()) if(id.empty())
{ {
@@ -1854,7 +1856,7 @@ void CloudViewer::addOrUpdateFrustum(
this->removeFrustum(id); this->removeFrustum(id);
#endif #endif
if(!transform.isNull()) if(!pose.isNull())
{ {
if(_frustums.find(id)==_frustums.end()) if(_frustums.find(id)==_frustums.end())
{ {
@@ -1865,9 +1867,9 @@ void CloudViewer::addOrUpdateFrustum(
frustumSize/=3; frustumSize/=3;
pcl::PointCloud<pcl::PointXYZ> frustumPoints; pcl::PointCloud<pcl::PointXYZ> frustumPoints;
frustumPoints.resize(frustumSize); frustumPoints.resize(frustumSize);
float scaleX = 0.5f * scale; float scaleX = tan((fovX>0?fovX:1.1)/2.0f) * scale;
float scaleY = 0.4f * scale; //4x3 arbitrary ratio float scaleY = tan((fovY>0?fovY:0.85)/2.0f) * scale;
float scaleZ = 0.3f * scale; float scaleZ = scale;
QColor c = Qt::gray; QColor c = Qt::gray;
if(color.isValid()) if(color.isValid())
{ {
@@ -1876,9 +1878,9 @@ void CloudViewer::addOrUpdateFrustum(
Transform opticalRotInv(0, -1, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0); Transform opticalRotInv(0, -1, 0, 0, 0, 0, -1, 0, 1, 0, 0, 0);
#if PCL_VERSION_COMPARE(<, 1, 7, 2) #if PCL_VERSION_COMPARE(<, 1, 7, 2)
Eigen::Affine3f t = (transform*localTransform*opticalRotInv).toEigen3f(); Eigen::Affine3f t = (pose*localTransform).toEigen3f();
#else #else
Eigen::Affine3f t = (localTransform*opticalRotInv).toEigen3f(); Eigen::Affine3f t = (localTransform).toEigen3f();
#endif #endif
for(int i=0; i<frustumSize; ++i) for(int i=0; i<frustumSize; ++i)
{ {
@@ -1902,7 +1904,7 @@ void CloudViewer::addOrUpdateFrustum(
_visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id); _visualizer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, c.alphaF(), id);
} }
#if PCL_VERSION_COMPARE(>=, 1, 7, 2) #if PCL_VERSION_COMPARE(>=, 1, 7, 2)
if(!this->updateFrustumPose(id, transform)) if(!this->updateFrustumPose(id, pose))
{ {
UERROR("Failed updating pose of frustum %s!?", id.c_str()); UERROR("Failed updating pose of frustum %s!?", id.c_str());
} }
@@ -2686,7 +2688,7 @@ void CloudViewer::updateCameraFrustums(const Transform & pose, const std::vector
} }
std::string id = uFormat("reference_frustum_%d", i); std::string id = uFormat("reference_frustum_%d", i);
this->removeFrustum(id); this->removeFrustum(id);
this->addOrUpdateFrustum(id, pose, baseToCamera, _frustumScale, _frustumColor); this->addOrUpdateFrustum(id, pose, baseToCamera, _frustumScale, _frustumColor, models[i].fovX(), models[i].fovY());
if(!baseToCamera.isIdentity()) if(!baseToCamera.isIdentity())
{ {
this->addOrUpdateLine(uFormat("reference_frustum_line_%d", i), pose, pose * baseToCamera, _frustumColor); this->addOrUpdateLine(uFormat("reference_frustum_line_%d", i), pose, pose * baseToCamera, _frustumColor);
guilib/src/DepthCalibrationDialog.cpp
+1 -1
View File
@@ -445,7 +445,7 @@ void DepthCalibrationDialog::calibrate(
for(std::map<int, SensorData>::iterator iter=sequence.begin(); iter!=sequence.end(); ++iter) for(std::map<int, SensorData>::iterator iter=sequence.begin(); iter!=sequence.end(); ++iter)
{ {
Transform baseToCamera = iter->second.cameraModels()[0].localTransform(); Transform baseToCamera = iter->second.cameraModels()[0].localTransform();
viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first), baseToCamera, 0.2); viewer->addOrUpdateFrustum(uFormat("frustum%d",iter->first), poses.at(iter->first), baseToCamera, 0.2, QColor(), iter->second.cameraModels()[0].fovX(), iter->second.cameraModels()[0].fovY());
} }
_progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)... done.").arg(map->size()).arg(sequence.size())); _progressDialog->appendText(tr("Viewing the cloud (%1 points and %2 poses)... done.").arg(map->size()).arg(sequence.size()));
guilib/src/MainWindow.cpp
+5 -4
View File
@@ -1274,7 +1274,7 @@ void MainWindow::processOdometry(const rtabmap::OdometryEvent & odom, bool dataI
if(!t.isNull()) if(!t.isNull())
{ {
QColor color = Qt::yellow; QColor color = Qt::yellow;
_cloudViewer->addOrUpdateFrustum(frustumId, _odometryCorrection*iter->second, t, _cloudViewer->getFrustumScale(), color); _cloudViewer->addOrUpdateFrustum(frustumId, _odometryCorrection*iter->second, t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
} }
} }
} }
@@ -2646,17 +2646,18 @@ void MainWindow::updateMapCloud(
// Supporting only one frustum per node // Supporting only one frustum per node
if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModel().isValidForProjection()) if(s.sensorData().cameraModels().size() == 1 || s.sensorData().stereoCameraModel().isValidForProjection())
{ {
Transform t = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().localTransform():s.sensorData().cameraModels()[0].localTransform(); const CameraModel & model = s.sensorData().stereoCameraModel().isValidForProjection()?s.sensorData().stereoCameraModel().left():s.sensorData().cameraModels()[0];
Transform t = model.localTransform();
if(!t.isNull()) if(!t.isNull())
{ {
QColor color = (Qt::GlobalColor)((mapId+3) % 12 + 7 ); QColor color = (Qt::GlobalColor)((mapId+3) % 12 + 7 );
_cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color); _cloudViewer->addOrUpdateFrustum(frustumId, iter->second, t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
if(_currentGTPosesMap.find(iter->first)!=_currentGTPosesMap.end()) if(_currentGTPosesMap.find(iter->first)!=_currentGTPosesMap.end())
{ {
std::string gtFrustumId = uFormat("f_gt_%d", iter->first); std::string gtFrustumId = uFormat("f_gt_%d", iter->first);
color = Qt::gray; color = Qt::gray;
_cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color); _cloudViewer->addOrUpdateFrustum(gtFrustumId, _currentGTPosesMap.at(iter->first), t, _cloudViewer->getFrustumScale(), color, model.fovX(), model.fovY());
} }
} }
} }