i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +08:00
Code Issues Packages Projects Releases Wiki Activity

Added stereo support for TYPE_8UC1 encoding and "Kannala Brandt4" (fisheye) distortion model (OpenLORIS).

Browse Source
This commit is contained in:
matlabbe
2021-04-02 19:08:58 -04:00
parent 56e1d6335e
commit 23c06875b9
4 changed files with 83 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
include/rtabmap_ros/GuiWrapper.h
+2
View File
@@ -47,6 +47,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
namespace rtabmap
{
class MainWindow;
class PreferencesDialog;
}
class QApplication;
@@ -112,6 +113,7 @@ private:
void processRequestedMap(const rtabmap_ros::MapData & map);
private:
rtabmap::PreferencesDialog * prefDialog_;
rtabmap::MainWindow * mainWindow_;
std::string cameraNodeName_;
double lastOdomInfoUpdateTime_;
src/GuiWrapper.cpp
+7 -2
View File
@@ -95,7 +95,8 @@ GuiWrapper::GuiWrapper(int & argc, char** argv) :
ROS_INFO("rtabmapviz: Using configuration from \"%s\"", configFile.toStdString().c_str());
uSleep(500);
mainWindow_ = new MainWindow(new PreferencesDialogROS(configFile));
prefDialog_ = new PreferencesDialogROS(configFile);
mainWindow_ = new MainWindow(prefDialog_);
mainWindow_->setWindowTitle(mainWindow_->windowTitle()+" [ROS]");
mainWindow_->show();
bool paused = false;
@@ -699,6 +700,10 @@ void GuiWrapper::commonStereoCallback(
{
lastOdomInfoUpdateTime_ = UTimer::now();
ParametersMap allParameters = prefDialog_->getAllParameters();
bool imagesAlreadyRectified = Parameters::defaultRtabmapImagesAlreadyRectified();
Parameters::parse(allParameters, Parameters::kRtabmapImagesAlreadyRectified(), imagesAlreadyRectified);
if(!rtabmap_ros::convertStereoMsg(
leftImageMsg,
rightImageMsg,
@@ -712,7 +717,7 @@ void GuiWrapper::commonStereoCallback(
stereoModel,
tfListener_,
waitForTransform_?waitForTransformDuration_:0.0,
true))
imagesAlreadyRectified))
{
ROS_ERROR("Could not convert stereo msgs! Aborting rtabmapviz update...");
return;
src/MsgConversion.cpp
+57 -12
View File
@@ -291,11 +291,13 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & imag
{
cv_bridge::CvImageConstPtr imageRectLeft = imageMsg;
cv_bridge::CvImageConstPtr imageRectRight = depthMsg;
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0))
@@ -321,8 +323,12 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & imag
}
cv::Mat left, right;
if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
{
left = imageRectLeft->image;
}
else if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
left = cv_bridge::cvtColor(imageRectLeft, "mono8")->image;
}
@@ -330,7 +336,15 @@ rtabmap::SensorData rgbdImageFromROS(const rtabmap_ros::RGBDImageConstPtr & imag
{
left = cv_bridge::cvtColor(imageRectLeft, "bgr8")->image;
}
right = cv_bridge::cvtColor(imageRectRight, "mono8")->image;
if(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
{
right = imageRectRight->image;
}
else
{
right = cv_bridge::cvtColor(imageRectRight, "mono8")->image;
}
//
@@ -781,7 +795,8 @@ rtabmap::CameraModel cameraModelFromROS(
{
if(camInfo.D.size()>=4 &&
(uStrContains(camInfo.distortion_model, "fisheye") ||
uStrContains(camInfo.distortion_model, "equidistant")))
uStrContains(camInfo.distortion_model, "equidistant") ||
uStrContains(camInfo.distortion_model, "Kannala Brandt4")))
{
D = cv::Mat::zeros(1, 6, CV_64FC1);
D.at<double>(0,0) = camInfo.D[0];
@@ -1888,13 +1903,15 @@ bool convertStereoMsg(
{
UASSERT(leftImageMsg.get() && rightImageMsg.get());
if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
if(!(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
!(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
!(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
@@ -1908,8 +1925,12 @@ bool convertStereoMsg(
return false;
}
if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
{
left = leftImageMsg->image;
}
else if(leftImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO16) == 0)
{
left = cv_bridge::cvtColor(leftImageMsg, "mono8")->image;
}
@@ -1917,7 +1938,15 @@ bool convertStereoMsg(
{
left = cv_bridge::cvtColor(leftImageMsg, "bgr8")->image;
}
right = cv_bridge::cvtColor(rightImageMsg, "mono8")->image;
if(rightImageMsg->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) == 0 ||
rightImageMsg->encoding.compare(sensor_msgs::image_encodings::MONO8) == 0)
{
right = rightImageMsg->image;
}
else
{
right = cv_bridge::cvtColor(rightImageMsg, "mono8")->image;
}
rtabmap::Transform localTransform = getTransform(frameId, leftImageMsg->header.frame_id, leftImageMsg->header.stamp, listener, waitForTransform);
if(localTransform.isNull())
@@ -1945,7 +1974,23 @@ bool convertStereoMsg(
}
}
stereoModel = rtabmap_ros::stereoCameraModelFromROS(leftCamInfoMsg, rightCamInfoMsg, localTransform);
rtabmap::Transform stereoTransform;
if(!alreadyRectified)
{
stereoTransform = getTransform(
rightCamInfoMsg.header.frame_id,
leftCamInfoMsg.header.frame_id,
leftCamInfoMsg.header.stamp,
listener,
waitForTransform);
if(stereoTransform.isNull())
{
ROS_ERROR("Parameter %s is false but we cannot get TF between the two cameras!", rtabmap::Parameters::kRtabmapImagesAlreadyRectified().c_str());
return false;
}
}
stereoModel = rtabmap_ros::stereoCameraModelFromROS(leftCamInfoMsg, rightCamInfoMsg, localTransform, stereoTransform);
if(stereoModel.baseline() > 10.0)
{
src/nodelets/stereo_odometry.cpp
+17 -6
View File
@@ -168,13 +168,15 @@ private:
callbackCalled();
if(!this->isPaused())
{
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
@@ -268,7 +270,9 @@ private:
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":
keepColor_ && imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0?"bgr8":"mono8");
cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight, "mono8");
cv_bridge::CvImagePtr ptrImageRight = cv_bridge::toCvCopy(imageRectRight,
imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1)==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8)==0?"":"mono8");
UTimer stepTimer;
//
@@ -301,13 +305,15 @@ private:
cv_bridge::CvImageConstPtr imageRectLeft, imageRectRight;
rtabmap_ros::toCvShare(image, imageRectLeft, imageRectRight);
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
if(!(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::BGRA8) == 0 ||
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::RGBA8) == 0) ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
!(imageRectRight->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO8) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::MONO16) ==0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::BGR8) == 0 ||
imageRectRight->encoding.compare(sensor_msgs::image_encodings::RGB8) == 0 ||
@@ -413,7 +419,12 @@ private:
ptrImageLeft = cv_bridge::cvtColor(imageRectLeft, "mono8");
}
}
cv_bridge::CvImagePtr ptrImageRight = cv_bridge::cvtColor(imageRectRight, "mono8");
cv_bridge::CvImageConstPtr ptrImageRight = imageRectRight;
if(imageRectLeft->encoding.compare(sensor_msgs::image_encodings::TYPE_8UC1) !=0 &&
imageRectLeft->encoding.compare(sensor_msgs::image_encodings::MONO8) != 0)
{
ptrImageRight = cv_bridge::cvtColor(imageRectRight, "mono8");
}
UTimer stepTimer;
//