i/rtabmap_ros
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap_ros.git synced 2026-10-03 16:27:46 +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::fromROSMsg(scanOut, *pclScan);
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;
}
else
@@ -2110,7 +2110,7 @@ bool convertScanMsg(
pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
pcl::fromROSMsg(scanOut, *pclScan);
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;
}
@@ -2217,7 +2217,7 @@ bool convertScan3dMsg(
{
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)
{
@@ -2227,7 +2227,7 @@ bool convertScan3dMsg(
{
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
{
@@ -2237,7 +2237,7 @@ bool convertScan3dMsg(
{
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
@@ -2250,7 +2250,7 @@ bool convertScan3dMsg(
{
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)
{
@@ -2260,7 +2260,7 @@ bool convertScan3dMsg(
{
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
{
@@ -2270,7 +2270,7 @@ bool convertScan3dMsg(
{
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;
src/nodelets/icp_odometry.cpp
+10 -12
View File
@@ -339,7 +339,7 @@ private:
pclScan->is_dense = true;
}
cv::Mat scan;
LaserScan scan;
int maxLaserScans = (int)scanMsg->ranges.size();
if(!pclScan->empty() || !pclScanI->empty())
{
@@ -401,7 +401,7 @@ private:
}
}
pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanINormal;
pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal;
pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal;
if(hasIntensity)
{
pclScanINormal.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
@@ -410,7 +410,7 @@ private:
}
else
{
pclScanNormal.reset(new pcl::PointCloud<pcl::PointXYZINormal>);
pclScanNormal.reset(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
}
@@ -459,10 +459,9 @@ private:
}
rtabmap::SensorData data(
LaserScan(scan, maxLaserScans, scanMsg->range_max,
scan.channels()==6?LaserScan::kXYINormal:
scan.channels()==5?LaserScan::kXYNormal:
scan.channels()==3?LaserScan::kXYI:LaserScan::kXY,
LaserScan(scan,
maxLaserScans,
scanMsg->range_max,
localScanTransform),
cv::Mat(),
cv::Mat(),
@@ -516,7 +515,7 @@ private:
cloudMsg = *pointCloudMsg;
}
cv::Mat scan;
LaserScan scan;
bool hasNormals = false;
bool hasIntensity = false;
for(unsigned int i=0; i<cloudMsg.fields.size(); ++i)
@@ -738,10 +737,9 @@ private:
}
}
LaserScan laserScan(scan, maxLaserScans, 0,
scan.channels()==7?LaserScan::kXYZINormal:
scan.channels()==6?LaserScan::kXYZNormal:
scan.channels()==4?LaserScan::kXYZI:LaserScan::kXYZ,
LaserScan laserScan(scan,
maxLaserScans,
0,
localScanTransform);
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");
cv_bridge::CvImagePtr ptrDepth = cv_bridge::toCvCopy(depth);
cv::Mat scan;
LaserScan scan;
Transform localScanTransform = Transform::getIdentity();
int maxLaserScans = 0;
if(scanMsg.get() != 0)
@@ -408,7 +408,7 @@ private:
}
rtabmap::SensorData data(
LaserScan::backwardCompatibility(scan,
LaserScan(scan,
scanMsg.get() != 0 || cloudMsg.get() != 0?maxLaserScans:0,
scanMsg.get() != 0?scanMsg->range_max:0,
localScanTransform),