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

Updated with latest rtabmap changes.

Browse Source
This commit is contained in:
matlabbe
2021-02-07 17:28:43 -05:00
parent 5a0ddaf9f5
commit 1f5239245c
3 changed files with 20 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/MsgConversion.cpp
+8 -8
View File
@@ -2102,7 +2102,7 @@ bool convertScanMsg(
pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>); pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
pcl::fromROSMsg(scanOut, *pclScan); pcl::fromROSMsg(scanOut, *pclScan);
pclScan->is_dense = true; pclScan->is_dense = true;
data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom).data(); // put back in laser frame
format = rtabmap::LaserScan::kXYI; format = rtabmap::LaserScan::kXYI;
} }
else else
@@ -2110,7 +2110,7 @@ bool convertScanMsg(
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>); pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan); pcl::fromROSMsg(scanOut, *pclScan);
pclScan->is_dense = true; pclScan->is_dense = true;
data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom); // put back in laser frame data = rtabmap::util3d::laserScan2dFromPointCloud(*pclScan, laserToOdom).data(); // put back in laser frame
format = rtabmap::LaserScan::kXY; format = rtabmap::LaserScan::kXY;
} }
@@ -2217,7 +2217,7 @@ bool convertScan3dMsg(
{ {
pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan); pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
} }
scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, rtabmap::LaserScan::kXYZRGBNormal, scanLocalTransform); scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
} }
else if(hasIntensity) else if(hasIntensity)
{ {
@@ -2227,7 +2227,7 @@ bool convertScan3dMsg(
{ {
pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan); pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
} }
scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, rtabmap::LaserScan::kXYZINormal, scanLocalTransform); scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
} }
else else
{ {
@@ -2237,7 +2237,7 @@ bool convertScan3dMsg(
{ {
pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan); pclScan = rtabmap::util3d::removeNaNNormalsFromPointCloud(pclScan);
} }
scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, rtabmap::LaserScan::kXYZNormal, scanLocalTransform); scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
} }
} }
else else
@@ -2250,7 +2250,7 @@ bool convertScan3dMsg(
{ {
pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan); pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
} }
scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, rtabmap::LaserScan::kXYZRGB, scanLocalTransform); scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
} }
else if(hasIntensity) else if(hasIntensity)
{ {
@@ -2260,7 +2260,7 @@ bool convertScan3dMsg(
{ {
pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan); pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
} }
scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, rtabmap::LaserScan::kXYZI, scanLocalTransform); scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
} }
else else
{ {
@@ -2270,7 +2270,7 @@ bool convertScan3dMsg(
{ {
pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan); pclScan = rtabmap::util3d::removeNaNFromPointCloud(pclScan);
} }
scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, rtabmap::LaserScan::kXYZ, scanLocalTransform); scan = rtabmap::LaserScan(rtabmap::util3d::laserScanFromPointCloud(*pclScan), maxPoints, maxRange, scanLocalTransform);
} }
} }
return true; return true;
src/nodelets/icp_odometry.cpp
+10 -12
View File
@@ -339,7 +339,7 @@ private:
pclScan->is_dense = true; pclScan->is_dense = true;
} }
cv::Mat scan; LaserScan scan;
int maxLaserScans = (int)scanMsg->ranges.size(); int maxLaserScans = (int)scanMsg->ranges.size();
if(!pclScan->empty() || !pclScanI->empty()) if(!pclScan->empty() || !pclScanI->empty())
{ {
@@ -401,7 +401,7 @@ private:
} }
} }
pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanINormal; pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanINormal;
pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal; pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal;
if(hasIntensity) if(hasIntensity)
{ {
pclScanINormal.reset(new pcl::PointCloud<pcl::PointXYZINormal>); pclScanINormal.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
@@ -410,7 +410,7 @@ private:
} }
else else
{ {
pclScanNormal.reset(new pcl::PointCloud<pcl::PointXYZINormal>); pclScanNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal); pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal); scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
} }
@@ -459,10 +459,9 @@ private:
} }
rtabmap::SensorData data( rtabmap::SensorData data(
LaserScan(scan, maxLaserScans, scanMsg->range_max, LaserScan(scan,
scan.channels()==6?LaserScan::kXYINormal: maxLaserScans,
scan.channels()==5?LaserScan::kXYNormal: scanMsg->range_max,
scan.channels()==3?LaserScan::kXYI:LaserScan::kXY,
localScanTransform), localScanTransform),
cv::Mat(), cv::Mat(),
cv::Mat(), cv::Mat(),
@@ -516,7 +515,7 @@ private:
cloudMsg = *pointCloudMsg; cloudMsg = *pointCloudMsg;
} }
cv::Mat scan; LaserScan scan;
bool hasNormals = false; bool hasNormals = false;
bool hasIntensity = false; bool hasIntensity = false;
for(unsigned int i=0; i<cloudMsg.fields.size(); ++i) for(unsigned int i=0; i<cloudMsg.fields.size(); ++i)
@@ -738,10 +737,9 @@ private:
} }
} }
LaserScan laserScan(scan, maxLaserScans, 0, LaserScan laserScan(scan,
scan.channels()==7?LaserScan::kXYZINormal: maxLaserScans,
scan.channels()==6?LaserScan::kXYZNormal: 0,
scan.channels()==4?LaserScan::kXYZI:LaserScan::kXYZ,
localScanTransform); localScanTransform);
if(scanRangeMin_ > 0 || scanRangeMax_ > 0) if(scanRangeMin_ > 0 || scanRangeMax_ > 0)
{ {
src/nodelets/rgbdicp_odometry.cpp
+2 -2
View File
@@ -286,7 +286,7 @@ private:
keepColor_ && image->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0?"bgr8":"mono8"); keepColor_ && image->encoding.compare(sensor_msgs::image_encodings::MONO16)!=0?"bgr8":"mono8");
cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth); cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
cv::Mat scan; LaserScan scan;
Transform localScanTransform = Transform::getIdentity(); Transform localScanTransform = Transform::getIdentity();
int maxLaserScans = 0; int maxLaserScans = 0;
if(scanMsg.get() != 0) if(scanMsg.get() != 0)
@@ -408,7 +408,7 @@ private:
} }
rtabmap::SensorData data( rtabmap::SensorData data(
LaserScan::backwardCompatibility(scan, LaserScan(scan,
scanMsg.get() != 0 || cloudMsg.get() != 0?maxLaserScans:0, scanMsg.get() != 0 || cloudMsg.get() != 0?maxLaserScans:0,
scanMsg.get() != 0?scanMsg->range_max:0, scanMsg.get() != 0?scanMsg->range_max:0,
localScanTransform), localScanTransform),