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

Updated for rtabmap 0.19.1. Added VINS example in euroc_datasets.launch

Browse Source
This commit is contained in:
matlabbe
2019-03-18 20:47:13 -04:00
parent 6bd420bc54
commit ab23e651bf
8 changed files with 91 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+33 -2
View File
@@ -700,13 +700,44 @@ void cameraModelToROS(
rtabmap::StereoCameraModel stereoCameraModelFromROS(
const sensor_msgs::CameraInfo & leftCamInfo,
const sensor_msgs::CameraInfo & rightCamInfo,
const rtabmap::Transform & localTransform)
const rtabmap::Transform & localTransform,
const rtabmap::Transform & stereoTransform)
{
return rtabmap::StereoCameraModel(
"ros",
cameraModelFromROS(leftCamInfo, localTransform),
cameraModelFromROS(rightCamInfo, localTransform),
rtabmap::Transform());
stereoTransform);
}
rtabmap::StereoCameraModel stereoCameraModelFromROS(
const sensor_msgs::CameraInfo & leftCamInfo,
const sensor_msgs::CameraInfo & rightCamInfo,
const std::string & frameId,
tf::TransformListener & listener,
double waitForTransform)
{
rtabmap::Transform localTransform = getTransform(
frameId,
leftCamInfo.header.frame_id,
leftCamInfo.header.stamp,
listener,
waitForTransform);
if(localTransform.isNull())
{
return rtabmap::StereoCameraModel();
}
rtabmap::Transform stereoTransform = getTransform(
leftCamInfo.header.frame_id,
rightCamInfo.header.frame_id,
leftCamInfo.header.stamp,
listener,
waitForTransform);
if(stereoTransform.isNull())
{
return rtabmap::StereoCameraModel();
}
return stereoCameraModelFromROS(leftCamInfo, rightCamInfo, localTransform, stereoTransform);
}
void mapDataFromROS(
src/OdometryROS.cpp
+7 -2
View File
@@ -343,7 +343,10 @@ void OdometryROS::onInit()
odomStrategy_ = 0;
Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy_);
if(waitIMUToinit_ || odomStrategy_ == Odometry::kTypeMSCKF || odomStrategy_ == Odometry::kTypeOkvis)
if(waitIMUToinit_ ||
odomStrategy_ == Odometry::kTypeMSCKF ||
odomStrategy_ == Odometry::kTypeOkvis ||
odomStrategy_ == Odometry::kTypeVINS)
{
int queueSize = 10;
pnh.param("queue_size", queueSize, queueSize);
@@ -414,6 +417,7 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
{
if(odomStrategy_ != Odometry::kTypeOkvis &&
odomStrategy_ != Odometry::kTypeMSCKF &&
odomStrategy_ != Odometry::kTypeVINS &&
!odometry_->getPose().isIdentity())
{
// For non-inertial odometry approaches, IMU is only used to initialize the initial orientation below
@@ -441,7 +445,8 @@ void OdometryROS::callbackIMU(const sensor_msgs::ImuConstPtr& msg)
localTransform);
if(odomStrategy_ != Odometry::kTypeOkvis &&
odomStrategy_ != Odometry::kTypeMSCKF)
odomStrategy_ != Odometry::kTypeMSCKF &&
odomStrategy_ != Odometry::kTypeVINS)
{
if(!odometry_->getPose().isIdentity())
{
src/nodelets/stereo_odometry.cpp
+20 -3
View File
@@ -194,10 +194,27 @@ private:
int quality = -1;
if(imageRectLeft->data.size() && imageRectRight->data.size())
{
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform);
if(stereoModel.baseline() <= 0)
bool alreadyRectified = true;
Parameters::parse(parameters(), Parameters::kRtabmapImagesAlreadyRectified(), alreadyRectified);
rtabmap::Transform stereoTransform;
if(!alreadyRectified)
{
NODELET_FATAL("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
stereoTransform = getTransform(
cameraInfoLeft->header.frame_id,
cameraInfoRight->header.frame_id,
cameraInfoLeft->header.stamp);
if(stereoTransform.isNull())
{
NODELET_ERROR("Parameter %s is false but we cannot get TF between the two cameras!", Parameters::kRtabmapImagesAlreadyRectified().c_str());
return;
}
}
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*cameraInfoLeft, *cameraInfoRight, localTransform, stereoTransform);
if(alreadyRectified && stereoModel.baseline() <= 0)
{
NODELET_ERROR("The stereo baseline (%f) should be positive (baseline=-Tx/fx). We assume a horizontal left/right stereo "
"setup where the Tx (or P(0,3)) is negative in the right camera info msg.", stereoModel.baseline());
return;
}