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

0.17.4 (added optional imu input topic to stereo odometry when a vio approach is selected, see Odom/Strategy parameter)

Browse Source
This commit is contained in:
matlabbe
2018-07-23 15:58:51 -04:00
parent 08972a5a32
commit 7a648a83d4
8 changed files with 151 additions and 35 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+35 -25
View File
@@ -192,6 +192,7 @@ void OdometryROS::onInit()
//parameters
parameters_ = Parameters::getDefaultOdometryParameters(stereoParams_, visParams_, icpParams_);
parameters_.insert(*Parameters::getDefaultParameters().find(Parameters::kRtabmapImagesAlreadyRectified()));
if(!configPath.empty())
{
if(UFile::exists(configPath.c_str()))
@@ -377,24 +378,27 @@ Transform OdometryROS::getTransform(const std::string & fromFrameId, const std::
void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
{
if(odometry_->getPose().isIdentity() &&
!groundTruthFrameId_.empty())
if(!data.imageRaw().empty())
{
// sync with the first value of the ground truth
Transform initialPose = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
if(initialPose.isNull())
if(odometry_->getPose().isIdentity() &&
!groundTruthFrameId_.empty())
{
NODELET_WARN("Ground truth frames \"%s\" -> \"%s\" are set but failed to "
"get them, odometry won't be synchronized with ground truth.",
groundTruthFrameId_.c_str(), groundTruthBaseFrameId_.c_str());
}
else
{
NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
initialPose.prettyPrint().c_str(),
groundTruthFrameId_.c_str(),
groundTruthBaseFrameId_.c_str());
odometry_->reset(initialPose);
// sync with the first value of the ground truth
Transform initialPose = getTransform(groundTruthFrameId_, groundTruthBaseFrameId_, stamp);
if(initialPose.isNull())
{
NODELET_WARN("Ground truth frames \"%s\" -> \"%s\" are set but failed to "
"get them, odometry won't be synchronized with ground truth.",
groundTruthFrameId_.c_str(), groundTruthBaseFrameId_.c_str());
}
else
{
NODELET_INFO( "Initializing odometry pose to %s (from \"%s\" -> \"%s\")",
initialPose.prettyPrint().c_str(),
groundTruthFrameId_.c_str(),
groundTruthBaseFrameId_.c_str());
odometry_->reset(initialPose);
}
}
}
@@ -617,6 +621,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odomLocalScanMap_.publish(cloudMsg);
}
}
else if(data.imageRaw().empty() && !data.imu().empty())
{
return;
}
else if(publishNullWhenLost_)
{
//NODELET_WARN( "Odometry lost!");
@@ -676,22 +684,24 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odomInfoPub_.publish(infoMsg);
}
if(visParams_)
if(!data.imageRaw().empty())
{
if(icpParams_)
if(visParams_)
{
NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
if(icpParams_)
{
NODELET_INFO( "Odom: quality=%d, ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.inliers, info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
}
else
{
NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
}
}
else
{
NODELET_INFO( "Odom: quality=%d, std dev=%fm|%frad, update time=%fs", info.reg.inliers, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
}
}
else
{
NODELET_INFO( "Odom: ratio=%f, std dev=%fm|%frad, update time=%fs", info.reg.icpInliersRatio, pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(0,0)), pose.isNull()?0.0f:std::sqrt(info.reg.covariance.at<double>(5,5)), (ros::WallTime::now()-time).toSec());
}
}
bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
src/nodelets/stereo_odometry.cpp
+42 -2
View File
@@ -38,6 +38,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
#include <sensor_msgs/Imu.h>
#include <image_geometry/stereo_camera_model.h>
@@ -49,6 +50,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UTimer.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/core/Odometry.h>
using namespace rtabmap;
@@ -140,6 +142,15 @@ private:
cameraInfoLeft_.getTopic().c_str(),
cameraInfoRight_.getTopic().c_str());
}
int odomStrategy = 0;
Parameters::parse(this->parameters(), Parameters::kOdomStrategy(), odomStrategy);
if(odomStrategy == Odometry::kTypeOkvis || odomStrategy == Odometry::kTypeMSCKF)
{
imuSub_ = nh.subscribe("imu", queueSize_*5, &StereoOdometry::callbackIMU, this);
NODELET_INFO("VIO approach selected, subscribing to IMU topic %s", imuSub_.getTopic().c_str());
}
this->startWarningThread(subscribedTopicsMsg, approxSync);
}
@@ -185,8 +196,6 @@ private:
return;
}
ros::WallTime time = ros::WallTime::now();
int quality = -1;
if(imageRectLeft->data.size() && imageRectRight->data.size())
{
@@ -312,6 +321,36 @@ private:
}
}
void callbackIMU(
const sensor_msgs::ImuConstPtr& msg)
{
if(!this->isPaused())
{
double stamp = msg->header.stamp.toSec();
rtabmap::Transform localTransform = rtabmap::Transform::getIdentity();
if(this->frameId().compare(msg->header.frame_id) != 0)
{
localTransform = getTransform(this->frameId(), msg->header.frame_id, msg->header.stamp);
}
if(localTransform.isNull())
{
ROS_ERROR("Could not transform IMU msg from frame \"%s\" to frame \"%s\", TF not available at time %f",
msg->header.frame_id.c_str(), this->frameId().c_str(), stamp);
return;
}
IMU imu(
cv::Vec3d(msg->angular_velocity.x, msg->angular_velocity.y, msg->angular_velocity.z),
cv::Mat(3,3,CV_64FC1,(void*)msg->angular_velocity_covariance.data()).clone(),
cv::Vec3d(msg->linear_acceleration.x, msg->linear_acceleration.y, msg->linear_acceleration.z),
cv::Mat(3,3,CV_64FC1,(void*)msg->linear_acceleration_covariance.data()).clone(),
localTransform);
SensorData data(imu, 0, stamp);
this->processData(data, msg->header.stamp);
}
}
protected:
virtual void flushCallbacks()
{
@@ -340,6 +379,7 @@ private:
typedef message_filters::sync_policies::ExactTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> MyExactSyncPolicy;
message_filters::Synchronizer<MyExactSyncPolicy> * exactSync_;
ros::Subscriber rgbdSub_;
ros::Subscriber imuSub_;
int queueSize_;
};