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

Updated for rtabmap 0.20.5

Browse Source
This commit is contained in:
matlabbe
2020-10-12 21:51:47 -04:00
parent e282586b5d
commit 905e8ada99
3 changed files with 36 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/OdometryROS.cpp
+5 -5
View File
@@ -674,14 +674,14 @@ void OdometryROS::processData(const SensorData & data, const rclcpp::Time & stam
// check which type of Odometry is using
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::vector<cv::Point3f> & words3 = ((OdometryF2M*)odometry_)->getLastFrame().getWords3();
if(words3.size())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::multimap<int, cv::Point3f>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
for(std::vector<cv::Point3f>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
{
// transform to odom frame
cv::Point3f pt = util3d::transformPoint(iter->second, pose);
cv::Point3f pt = util3d::transformPoint(*iter, pose);
cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
}
@@ -699,10 +699,10 @@ void OdometryROS::processData(const SensorData & data, const rclcpp::Time & stam
if(refFrame.getWords3().size())
{
pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::multimap<int, cv::Point3f>::const_iterator iter=refFrame.getWords3().begin(); iter!=refFrame.getWords3().end(); ++iter)
for(std::vector<cv::Point3f>::const_iterator iter=refFrame.getWords3().begin(); iter!=refFrame.getWords3().end(); ++iter)
{
// transform to odom frame
cv::Point3f pt = util3d::transformPoint(iter->second, pose);
cv::Point3f pt = util3d::transformPoint(*iter, pose);
cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
}
sensor_msgs::msg::PointCloud2 cloudMsg;