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 package to rtabmap 0.8.4. Fixed not saved database on exit (bug introduced with the last commit).

Browse Source
This commit is contained in:
Mathieu Labbe
2015-02-24 16:08:08 -05:00
parent f52c34746e
commit 6020b60926
11 changed files with 129 additions and 39 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/CoreWrapper.cpp
+39 -12
View File
@@ -70,10 +70,17 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
using namespace rtabmap;
float max3( const float& a, const float& b, const float& c)
{
float m=a>b?a:b;
return m>c?m:c;
}
CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
paused_(false),
lastPose_(Transform::getIdentity()),
variance_(0),
rotVariance_(0),
transVariance_(0),
frameId_("base_link"),
mapFrameId_("map"),
odomFrameId_(""),
@@ -99,6 +106,11 @@ CoreWrapper::CoreWrapper(bool deleteDbOnStart) :
stereoScanSync_(0),
stereoApproxSync_(0),
stereoExactSync_(0),
depthTFSync_(0),
depthScanTFSync_(0),
stereoScanTFSync_(0),
stereoApproxTFSync_(0),
stereoExactTFSync_(0),
transformThread_(0),
rate_(Parameters::defaultRtabmapDetectionRate()),
time_(ros::Time::now()),
@@ -537,13 +549,20 @@ bool CoreWrapper::commonOdomUpdate(const nav_msgs::OdometryConstPtr & odomMsg)
{
UWARN("Odometry is reset (identity pose detected). Increment map id!");
rtabmap_.triggerNewMap();
variance_ = 0;
rotVariance_ = 0;
transVariance_ = 0;
}
lastPose_ = odom;
if(odomMsg->pose.covariance[0] > variance_)
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
if(rotVariance > rotVariance_)
{
variance_ = odomMsg->pose.covariance[0];
rotVariance_ = rotVariance;
}
if(transVariance > transVariance_)
{
transVariance_ = transVariance;
}
// Throttle
@@ -591,7 +610,8 @@ bool CoreWrapper::commonOdomTFUpdate(const ros::Time & stamp)
{
UWARN("Odometry is reset (identity pose detected). Increment map id!");
rtabmap_.triggerNewMap();
variance_ = 0;
rotVariance_ = 0;
transVariance_ = 0;
}
lastPose_ = odom;
@@ -700,7 +720,8 @@ void CoreWrapper::commonDepthCallback(
ptrImage->image,
lastPose_,
odomFrameId,
variance_>0?variance_:1.0f,
rotVariance_>0?rotVariance_:1.0f,
transVariance_>0?transVariance_:1.0f,
ptrDepth->image,
fx,
fy,
@@ -708,7 +729,8 @@ void CoreWrapper::commonDepthCallback(
cy,
localTransform,
scan);
variance_ = 0;
rotVariance_ = 0;
transVariance_ = 0;
}
void CoreWrapper::commonStereoCallback(
@@ -779,7 +801,8 @@ void CoreWrapper::commonStereoCallback(
ptrLeftImage->image,
lastPose_,
odomFrameId,
variance_>0?variance_:1.0f,
rotVariance_>0?rotVariance_:1.0f,
transVariance_>0?transVariance_:1.0f,
ptrRightImage->image,
fx,
baseline,
@@ -787,7 +810,8 @@ void CoreWrapper::commonStereoCallback(
cy,
localTransform,
scan);
variance_ = 0;
rotVariance_ = 0;
transVariance_ = 0;
}
void CoreWrapper::depthCallback(
@@ -905,7 +929,8 @@ void CoreWrapper::process(
const cv::Mat & image,
const Transform & odom,
const std::string & odomFrameId,
float odomVariance,
float odomRotationalVariance,
float odomTransitionalVariance,
const cv::Mat & depthOrRightImage,
float fx,
float fyOrBaseline,
@@ -963,7 +988,8 @@ void CoreWrapper::process(
cy,
localTransform,
odom,
odomVariance,
odomRotationalVariance,
odomTransitionalVariance,
id);
if(rtabmap_.process(data))
@@ -1227,7 +1253,8 @@ bool CoreWrapper::resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empt
{
ROS_INFO("rtabmap: Reset");
rtabmap_.resetMemory();
variance_ = 0;
rotVariance_ = 0;
transVariance_ = 0;
lastPose_.setIdentity();
currentMetricGoal_.setNull();
return true;
src/CoreWrapper.h
+4 -2
View File
@@ -166,7 +166,8 @@ private:
const cv::Mat & image,
const rtabmap::Transform & odom = rtabmap::Transform(),
const std::string & odomFrameId = "",
float odomVariance = 1.0f,
float odomRotationalVariance = 1.0f,
float odomTransitionalVariance = 1.0f,
const cv::Mat & depthOrRightImage = cv::Mat(),
float fx = 0.0f,
float fyOrBaseline = 0.0f,
@@ -215,7 +216,8 @@ private:
rtabmap::Rtabmap rtabmap_;
bool paused_;
rtabmap::Transform lastPose_;
float variance_;
float rotVariance_;
float transVariance_;
rtabmap::Transform currentMetricGoal_;
std::string frameId_;
src/DataRecorderNode.cpp
+28 -5
View File
@@ -63,6 +63,11 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
using namespace rtabmap;
float max3( const float& a, const float& b, const float& c)
{
float m=a>b?a:b;
return m>c?m:c;
}
class DataRecorderWrapper
{
@@ -283,7 +288,9 @@ private:
cy,
localTransform,
Transform(),
1.0f);
1.0f,
1.0f,
0);
recorder_.addData(data);
}
@@ -352,6 +359,9 @@ private:
depth16 = ptrDepth->image.clone();
}
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
rtabmap::SensorData data(
ptrImage->image.clone(),
depth16,
@@ -361,7 +371,9 @@ private:
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0]>0?odomMsg->pose.covariance[0]:1.0f);
rotVariance>0?rotVariance:1.0f,
transVariance>0?transVariance:1.0f,
0);
recorder_.addData(data);
}
@@ -445,6 +457,9 @@ private:
depth16 = ptrDepth->image.clone();
}
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
rtabmap::SensorData data(
scan,
ptrImage->image.clone(),
@@ -455,7 +470,9 @@ private:
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0]>0?odomMsg->pose.covariance[0]:1.0f);
rotVariance>0?rotVariance:1.0f,
transVariance>0?transVariance:1.0f,
0);
recorder_.addData(data);
}
@@ -499,6 +516,8 @@ private:
float cx = model.right().cx();
float cy = model.right().cy();
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
rtabmap::SensorData data(
ptrLeftImage->image.clone(),
ptrRightImage->image.clone(),
@@ -508,7 +527,9 @@ private:
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0]>0?odomMsg->pose.covariance[0]:1.0f);
rotVariance>0?rotVariance:1.0f,
transVariance>0?transVariance:1.0f,
0);
recorder_.addData(data);
}
@@ -560,7 +581,9 @@ private:
cy,
localTransform,
Transform(),
1.0f);
1.0f,
1.0f,
0);
recorder_.addData(data);
}
src/DbPlayerNode.cpp
+6 -6
View File
@@ -252,12 +252,12 @@ int main(int argc, char** argv)
odom.header.frame_id = odomFrameId;
odom.header.stamp = time;
rtabmap_ros::transformToPoseMsg(data.pose(), odom.pose.pose);
odom.pose.covariance[0] = data.poseVariance();
odom.pose.covariance[7] = data.poseVariance();
odom.pose.covariance[14] = data.poseVariance();
odom.pose.covariance[21] = data.poseVariance();
odom.pose.covariance[28] = data.poseVariance();
odom.pose.covariance[35] = data.poseVariance();
odom.pose.covariance[0] = data.poseTransVariance();
odom.pose.covariance[7] = data.poseTransVariance();
odom.pose.covariance[14] = data.poseTransVariance();
odom.pose.covariance[21] = data.poseRotVariance();
odom.pose.covariance[28] = data.poseRotVariance();
odom.pose.covariance[35] = data.poseRotVariance();
odometryPub.publish(odom);
}
}
src/GuiWrapper.cpp
+39 -7
View File
@@ -58,6 +58,12 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <pcl_conversions/pcl_conversions.h>
#include <laser_geometry/laser_geometry.h>
float max3( const float& a, const float& b, const float& c)
{
float m=a>b?a:b;
return m>c?m:c;
}
GuiWrapper::GuiWrapper(int & argc, char** argv) :
app_(0),
mainWindow_(0),
@@ -364,7 +370,9 @@ void GuiWrapper::defaultCallback(const nav_msgs::OdometryConstPtr & odomMsg)
{
Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
rtabmap::SensorData data(cv::Mat(), odomMsg->header.seq);
data.setPose(odom, odomMsg->pose.covariance[0]);
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
data.setPose(odom, rotVariance, transVariance);
this->post(new OdometryEvent(data));
}
@@ -407,6 +415,9 @@ void GuiWrapper::depthCallback(
float cx = model.cx();
float cy = model.cy();
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
rtabmap::SensorData image(
ptrImage->image.clone(),
ptrDepth->image.clone(),
@@ -416,7 +427,8 @@ void GuiWrapper::depthCallback(
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0],
rotVariance,
transVariance,
odomMsg->header.seq);
this->post(new OdometryEvent(image));
}
@@ -461,6 +473,9 @@ void GuiWrapper::depthOdomInfoCallback(
float cx = model.cx();
float cy = model.cy();
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
rtabmap::SensorData image(
ptrImage->image.clone(),
ptrDepth->image.clone(),
@@ -470,7 +485,8 @@ void GuiWrapper::depthOdomInfoCallback(
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0],
rotVariance,
transVariance,
odomMsg->header.seq);
OdometryInfo info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
this->post(new OdometryEvent(image, info));
@@ -525,6 +541,9 @@ void GuiWrapper::depthScanCallback(
float cx = model.cx();
float cy = model.cy();
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
rtabmap::SensorData image(
scan,
ptrImage->image.clone(),
@@ -535,7 +554,8 @@ void GuiWrapper::depthScanCallback(
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0],
rotVariance,
transVariance,
odomMsg->header.seq);
this->post(new OdometryEvent(image));
}
@@ -613,6 +633,9 @@ void GuiWrapper::stereoScanCallback(
float cy = model.left().cy();
float baseline = model.baseline();
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
rtabmap::SensorData image(
scan,
ptrLeftImage->image.clone(),
@@ -623,7 +646,8 @@ void GuiWrapper::stereoScanCallback(
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0],
rotVariance,
transVariance,
odomMsg->header.seq);
this->post(new OdometryEvent(image));
}
@@ -692,6 +716,9 @@ void GuiWrapper::stereoOdomInfoCallback(
float cy = model.left().cy();
float baseline = model.baseline();
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
rtabmap::SensorData image(
ptrLeftImage->image.clone(),
ptrRightImage->image.clone(),
@@ -701,7 +728,8 @@ void GuiWrapper::stereoOdomInfoCallback(
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0],
rotVariance,
transVariance,
odomMsg->header.seq);
OdometryInfo info = rtabmap_ros::odomInfoFromROS(*odomInfoMsg);
this->post(new OdometryEvent(image, info));
@@ -770,6 +798,9 @@ void GuiWrapper::stereoCallback(
float cy = model.left().cy();
float baseline = model.baseline();
float transVariance = max3(odomMsg->pose.covariance[0], odomMsg->pose.covariance[7], odomMsg->pose.covariance[14]);
float rotVariance = max3(odomMsg->pose.covariance[21], odomMsg->pose.covariance[28], odomMsg->pose.covariance[35]);
rtabmap::SensorData image(
ptrLeftImage->image.clone(),
ptrRightImage->image.clone(),
@@ -779,7 +810,8 @@ void GuiWrapper::stereoCallback(
cy,
localTransform,
rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose),
odomMsg->pose.covariance[0],
rotVariance,
transVariance,
odomMsg->header.seq);
this->post(new OdometryEvent(image));
}
src/MsgConversion.cpp
+3 -2
View File
@@ -200,7 +200,7 @@ void infoToROS(const rtabmap::Statistics & stats, rtabmap_ros::Info & info)
rtabmap::Link linkFromROS(const rtabmap_ros::Link & msg)
{
return rtabmap::Link(msg.fromId, msg.toId, (rtabmap::Link::Type)msg.type, transformFromGeometryMsg(msg.transform), msg.variance);
return rtabmap::Link(msg.fromId, msg.toId, (rtabmap::Link::Type)msg.type, transformFromGeometryMsg(msg.transform), msg.rotVariance, msg.transVariance);
}
void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg)
@@ -208,7 +208,8 @@ void linkToROS(const rtabmap::Link & link, rtabmap_ros::Link & msg)
msg.fromId = link.from();
msg.toId = link.to();
msg.type = link.type();
msg.variance = link.variance();
msg.rotVariance = link.rotVariance();
msg.transVariance = link.transVariance();
transformToGeometryMsg(link.transform(), msg.transform);
}
src/RGBDOdometryNode.cpp
+3 -1
View File
@@ -147,7 +147,9 @@ public:
cy,
rtabmap_ros::transformFromTF(localTransform),
rtabmap::Transform(),
1.0f);
1.0f,
1.0f,
0);
this->processData(data, image->header);
}
src/StereoOdometryNode.cpp
+3 -1
View File
@@ -171,7 +171,9 @@ public:
cy,
rtabmap_ros::transformFromTF(localTransform),
rtabmap::Transform(),
1.0f);
1.0f,
1.0f,
0);
this->processData(data, imageRectLeft->header);
}