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

Fixed build with latest rtabmap 0.11

Browse Source
This commit is contained in:
matlabbe
2016-01-04 19:04:19 -05:00
parent b8307449bd
commit 451fdd1ec8
8 changed files with 89 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CameraNode.cpp
+1 -1
View File
@@ -193,7 +193,7 @@ public:
if(!path.empty() && UDirectory::exists(path))
{
//images
camera_ = new rtabmap::CameraImages(path, 1, false, false, false, frameRate);
camera_ = new rtabmap::CameraImages(path, frameRate);
}
else if(!path.empty() && UFile::exists(path))
{
src/CoreWrapper.cpp
+8 -24
View File
@@ -54,7 +54,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl_conversions/pcl_conversions.h>
#include <laser_geometry/laser_geometry.h>
#include <image_geometry/stereo_camera_model.h>
#ifdef WITH_OCTOMAP
#include <octomap_msgs/conversions.h>
@@ -817,23 +816,16 @@ void CoreWrapper::commonDepthCallback(
return;
}
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsgs[i]);
cameraModels.push_back(rtabmap::CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
localTransform));
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform));
if(scan2dMsg.get() == 0 && genScan_)
{
scanCloud2d += util3d::laserScanFromDepthImage(
subDepth,
model.fx(),
model.fy(),
model.cx(),
model.cy(),
cameraModels.back().fx(),
cameraModels.back().fy(),
cameraModels.back().cx(),
cameraModels.back().cy(),
genScanMaxDepth_,
localTransform);
genMaxScanPts += subDepth.cols;
@@ -1009,17 +1001,9 @@ void CoreWrapper::commonStereoCallback(
}
ptrRightImage = cv_bridge::toCvShare(rightImageMsg, "mono8");
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg);
rtabmap::StereoCameraModel stereoModel(
model.left().fx(),
model.left().fy(),
model.left().cx(),
model.left().cy(),
model.baseline(),
localTransform);
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
if(model.baseline() > 10.0)
if(stereoModel.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
@@ -1027,7 +1011,7 @@ void CoreWrapper::commonStereoCallback(
ROS_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
model.baseline());
stereoModel.baseline());
shown = true;
}
}
src/GuiWrapper.cpp
+4 -22
View File
@@ -40,9 +40,6 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <opencv2/highgui/highgui.hpp>
#include <image_geometry/pinhole_camera_model.h>
#include <image_geometry/stereo_camera_model.h>
#include <rtabmap/gui/MainWindow.h>
#include <rtabmap/core/RtabmapEvent.h>
#include <rtabmap/core/Parameters.h>
@@ -692,14 +689,7 @@ void GuiWrapper::commonDepthCallback(
return;
}
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfoMsgs[i]);
cameraModels.push_back(rtabmap::CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
localTransform));
cameraModels.push_back(rtabmap_ros::cameraModelFromROS(*cameraInfoMsgs[i], localTransform));
}
}
@@ -862,17 +852,9 @@ void GuiWrapper::commonStereoCallback(
}
}
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*leftCamInfoMsg, *rightCamInfoMsg);
rtabmap::StereoCameraModel stereoModel(
model.left().fx(),
model.left().fy(),
model.left().cx(),
model.left().cy(),
model.baseline(),
localTransform);
rtabmap::StereoCameraModel stereoModel = rtabmap_ros::stereoCameraModelFromROS(*leftCamInfoMsg, *rightCamInfoMsg, localTransform);
if(model.baseline() > 10.0)
if(stereoModel.baseline() > 10.0)
{
static bool shown = false;
if(!shown)
@@ -880,7 +862,7 @@ void GuiWrapper::commonStereoCallback(
ROS_WARN("Detected baseline (%f m) is quite large! Is your "
"right camera_info P(0,3) correctly set? Note that "
"baseline=-P(0,3)/P(0,0). This warning is printed only once.",
model.baseline());
stereoModel.baseline());
shown = true;
}
}
src/MsgConversion.cpp
+37 -4
View File
@@ -36,6 +36,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl_conversions/pcl_conversions.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf_conversions/tf_eigen.h>
#include <image_geometry/pinhole_camera_model.h>
#include <image_geometry/stereo_camera_model.h>
namespace rtabmap_ros {
@@ -293,6 +295,35 @@ void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ro
}
}
rtabmap::CameraModel cameraModelFromROS(
const sensor_msgs::CameraInfo & camInfo,
const rtabmap::Transform & localTransform)
{
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(camInfo);
return rtabmap::CameraModel(
model.fx(),
model.fy(),
model.cx(),
model.cy(),
localTransform);
}
rtabmap::StereoCameraModel stereoCameraModelFromROS(
const sensor_msgs::CameraInfo & leftCamInfo,
const sensor_msgs::CameraInfo & rightCamInfo,
const rtabmap::Transform & localTransform)
{
image_geometry::StereoCameraModel model;
model.fromCameraInfo(leftCamInfo, rightCamInfo);
return rtabmap::StereoCameraModel(
model.left().fx(),
model.left().fy(),
model.left().cx(),
model.left().cy(),
model.baseline(),
localTransform);
}
void mapDataFromROS(
const rtabmap_ros::MapData & msg,
std::map<int, rtabmap::Transform> & poses,
@@ -384,7 +415,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
{
//Features stuff...
std::multimap<int, cv::KeyPoint> words;
std::multimap<int, pcl::PointXYZ> words3D;
std::multimap<int, cv::Point3f> words3D;
pcl::PointCloud<pcl::PointXYZ> cloud;
if(msg.wordPts.data.size() &&
msg.wordPts.data.size() == msg.wordIds.size())
@@ -398,7 +429,7 @@ rtabmap::Signature nodeDataFromROS(const rtabmap_ros::NodeData & msg)
words.insert(std::make_pair(wordId, pt));
if(i< cloud.size())
{
words3D.insert(std::make_pair(wordId, cloud[i]));
words3D.insert(std::make_pair(wordId, cv::Point3f(cloud[i].x, cloud[i].y, cloud[i].z)));
}
}
@@ -539,11 +570,13 @@ void nodeDataToROS(const rtabmap::Signature & signature, rtabmap_ros::NodeData &
pcl::PointCloud<pcl::PointXYZ> cloud;
cloud.resize(signature.getWords3().size());
index = 0;
for(std::multimap<int, pcl::PointXYZ>::const_iterator jter=signature.getWords3().begin();
for(std::multimap<int, cv::Point3f>::const_iterator jter=signature.getWords3().begin();
jter!=signature.getWords3().end();
++jter)
{
cloud[index++] = jter->second;
cloud[index].x = jter->second.x;
cloud[index].y = jter->second.y;
cloud[index++].z = jter->second.z;
}
pcl::toROSMsg(cloud, msg.wordPts);
}
src/OdometryROS.cpp
+21 -15
View File
@@ -216,8 +216,8 @@ OdometryROS::OdometryROS(int argc, char * argv[], bool stereo) :
Parameters::parse(parameters_, Parameters::kOdomStrategy(), odomStrategy);
if(odomStrategy == 1)
{
ROS_INFO("Using OdometryOpticalFlow");
odometry_ = new rtabmap::OdometryOpticalFlow(parameters_);
ROS_INFO("Using OdometryF2F");
odometry_ = new rtabmap::OdometryF2F(parameters_);
}
else
{
@@ -369,13 +369,14 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
odomPub_.publish(odom);
}
// local map / reference frame
if(odomLocalMap_.getNumSubscribers() && dynamic_cast<OdometryBOW*>(odometry_))
{
const std::map<int, pcl::PointXYZ> & map = ((OdometryBOW*)odometry_)->getLocalMap();
pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::map<int, pcl::PointXYZ>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
const std::map<int, cv::Point3f> & map = ((OdometryBOW*)odometry_)->getLocalMap();
for(std::map<int, cv::Point3f>::const_iterator iter=map.begin(); iter!=map.end(); ++iter)
{
cloud.push_back(iter->second);
cloud.push_back(pcl::PointXYZ(iter->second.y, iter->second.y, iter->second.z));
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(cloud, cloudMsg);
@@ -391,13 +392,13 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
const rtabmap::Signature * s = ((OdometryBOW*)odometry_)->getMemory()->getLastWorkingSignature();
if(s)
{
const std::multimap<int, pcl::PointXYZ> & words3 = s->getWords3();
const std::multimap<int, cv::Point3f> & words3 = s->getWords3();
pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::multimap<int, pcl::PointXYZ>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
for(std::multimap<int, cv::Point3f>::const_iterator iter=words3.begin(); iter!=words3.end(); ++iter)
{
// transform to odom frame
pcl::PointXYZ pt = util3d::transformPoint(iter->second, pose);
cloud.push_back(pt);
cv::Point3f pt = util3d::transformPoint(iter->second, pose);
cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
}
sensor_msgs::PointCloud2 cloudMsg;
@@ -409,14 +410,19 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
}
else
{
//Optical flow
const pcl::PointCloud<pcl::PointXYZ>::Ptr & cloud = ((OdometryOpticalFlow*)odometry_)->getLastCorners3D();
if(cloud->size())
//Frame to Frame
const Signature & refFrame = ((OdometryF2F*)odometry_)->getRefFrame();
if(refFrame.getWords3().size())
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloudTransformed;
cloudTransformed = util3d::transformPointCloud(cloud, pose);
pcl::PointCloud<pcl::PointXYZ> cloud;
for(std::multimap<int, 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);
cloud.push_back(pcl::PointXYZ(pt.x, pt.y, pt.z));
}
sensor_msgs::PointCloud2 cloudMsg;
pcl::toROSMsg(*cloudTransformed, cloudMsg);
pcl::toROSMsg(cloud, cloudMsg);
cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
cloudMsg.header.frame_id = odomFrameId_;
odomLastFrame_.publish(cloudMsg);
src/nodelets/point_cloud_xyz.cpp
+5 -9
View File
@@ -29,6 +29,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
#include <rtabmap_ros/MsgConversion.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
@@ -238,18 +240,12 @@ private:
if(cloudPub_.getNumSubscribers())
{
image_geometry::PinholeCameraModel model;
model.fromCameraInfo(*cameraInfo);
float cx = model.cx();
float cy = model.cy();
pcl::PointCloud<pcl::PointXYZ>::Ptr pclCloud;
rtabmap::CameraModel leftModel = rtabmap_ros::cameraModelFromROS(*cameraInfo);
rtabmap::StereoCameraModel stereoModel(disparityMsg->f, disparityMsg->f, leftModel.cx(), leftModel.cy(), disparityMsg->T);
pclCloud = rtabmap::util3d::cloudFromDisparity(
disparity,
cx,
cy,
disparityMsg->f,
disparityMsg->T,
stereoModel,
decimation_);
processAndPublish(pclCloud, disparityMsg->header);
src/nodelets/point_cloud_xyzrgb.cpp
+3 -12
View File
@@ -33,6 +33,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <rtabmap_ros/MsgConversion.h>
#include <sensor_msgs/PointCloud2.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>
@@ -228,22 +230,11 @@ private:
}
ptrRightImage = cv_bridge::toCvShare(imageRight, "mono8");
image_geometry::StereoCameraModel model;
model.fromCameraInfo(*camInfoLeft, *camInfoRight);
float fx = model.left().fx();
float cx = model.left().cx();
float cy = model.left().cy();
float baseline = model.baseline();
pcl::PointCloud<pcl::PointXYZRGB>::Ptr pclCloud;
pclCloud = rtabmap::util3d::cloudFromStereoImages(
ptrLeftImage->image,
ptrRightImage->image,
cx,
cy,
fx,
baseline,
rtabmap_ros::stereoCameraModelFromROS(*camInfoLeft, *camInfoRight),
decimation_);
processAndPublish(pclCloud, imageLeft->header);