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 pull request https://github.com/introlab/rtabmap_ros/pull/104 to use Odometry::getType()

Browse Source
This commit is contained in:
matlabbe
2016-08-01 15:27:50 -04:00
parent 3f3b88975c
commit e5eba34d20
2 changed files with 3 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+3 -8
View File
@@ -426,7 +426,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
}
// local map / reference frame
if(odomLocalMap_.getNumSubscribers() && odometry_->isF2M())
if(odomLocalMap_.getNumSubscribers() && odometry_->getType() == Odometry::kTypeF2M)
{
pcl::PointCloud<pcl::PointXYZ> cloud;
const std::multimap<int, cv::Point3f> & map = ((OdometryF2M*)odometry_)->getMap().getWords3();
@@ -444,7 +444,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
if(odomLastFrame_.getNumSubscribers())
{
// check which type of Odometry is using
if(odometry_->isF2M()) // If it's Frame to Map Odometry
if(odometry_->getType() == Odometry::kTypeF2M) // If it's Frame to Map Odometry
{
const std::multimap<int, cv::Point3f> & words3 = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
if(words3.size())
@@ -464,7 +464,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odomLastFrame_.publish(cloudMsg);
}
}
else if(odometry_->isF2F()) // if Using Frame to Frame Odometry
else if(odometry_->getType() == Odometry::kTypeF2F) // if Using Frame to Frame Odometry
{
const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame();
@@ -550,11 +550,6 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
NODELET_INFO( "Odom: quality=%d, std dev=%fm, update time=%fs", info.inliers, pose.isNull()?0.0f:std::sqrt(info.variance), (ros::WallTime::now()-time).toSec());
}
bool OdometryROS::isOdometryF2M() const
{
return odometry_->isF2M();
}
bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{
NODELET_INFO( "visual_odometry: reset odom!");