i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-11 03:59:53 +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
include/rtabmap_ros/OdometryROS.h
-1
View File
@@ -70,7 +70,6 @@ public:
const rtabmap::ParametersMap & parameters() const {return parameters_;} const rtabmap::ParametersMap & parameters() const {return parameters_;}
const tf::TransformListener & tfListener() const {return tfListener_;} const tf::TransformListener & tfListener() const {return tfListener_;}
bool isPaused() const {return paused_;} bool isPaused() const {return paused_;}
bool isOdometryF2M() const;
rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const; rtabmap::Transform getTransform(const std::string & fromFrameId, const std::string & toFrameId, const ros::Time & stamp) const;
protected: protected:
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 // local map / reference frame
if(odomLocalMap_.getNumSubscribers() && odometry_->isF2M()) if(odomLocalMap_.getNumSubscribers() && odometry_->getType() == Odometry::kTypeF2M)
{ {
pcl::PointCloud<pcl::PointXYZ> cloud; pcl::PointCloud<pcl::PointXYZ> cloud;
const std::multimap<int, cv::Point3f> & map = ((OdometryF2M*)odometry_)->getMap().getWords3(); 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()) if(odomLastFrame_.getNumSubscribers())
{ {
// check which type of Odometry is using // 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(); const std::multimap<int, cv::Point3f> & words3 = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
if(words3.size()) if(words3.size())
@@ -464,7 +464,7 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odomLastFrame_.publish(cloudMsg); 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(); 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()); 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&) bool OdometryROS::reset(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
{ {
NODELET_INFO( "visual_odometry: reset odom!"); NODELET_INFO( "visual_odometry: reset odom!");