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 against latest changes from 0.11. Added localMap features to OdomInfo msg. Updated rgbdslam_datasets.launch.

Browse Source
This commit is contained in:
matlabbe
2016-03-09 17:31:57 -05:00
parent d712efe455
commit a4dfe5a21e
8 changed files with 82 additions and 23 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CMakeLists.txt
+1
View File
@@ -52,6 +52,7 @@ add_message_files(
Link.msg
OdomInfo.msg
Point2f.msg
Point3f.msg
Goal.msg
)
include/rtabmap_ros/MsgConversion.h
+7
View File
@@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap_ros/Link.h>
#include <rtabmap_ros/KeyPoint.h>
#include <rtabmap_ros/Point2f.h>
#include <rtabmap_ros/Point3f.h>
#include <rtabmap_ros/MapData.h>
#include <rtabmap_ros/MapGraph.h>
#include <rtabmap_ros/NodeData.h>
@@ -85,6 +86,12 @@ void point2fToROS(const cv::Point2f & kpt, rtabmap_ros::Point2f & msg);
std::vector<cv::Point2f> points2fFromROS(const std::vector<rtabmap_ros::Point2f> & msg);
void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ros::Point2f> & msg);
cv::Point3f point3fFromROS(const rtabmap_ros::Point3f & msg);
void point3fToROS(const cv::Point3f & kpt, rtabmap_ros::Point3f & msg);
std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg);
void points3fToROS(const std::vector<cv::Point3f> & kpts, std::vector<rtabmap_ros::Point3f> & msg);
rtabmap::CameraModel cameraModelFromROS(
const sensor_msgs::CameraInfo & camInfo,
const rtabmap::Transform & localTransform = rtabmap::Transform::getIdentity());
launch/tests/rgbdslam_datasets.launch
+9 -11
View File
@@ -29,21 +29,21 @@
<remap from="rgb/camera_info" to="/camera/rgb/camera_info"/>
<remap from="odom" to="vis_odom"/>
<param name="Odom/Strategy" type="string" value="0"/> <!-- 0=Frame-to-Map, 1=Frame-to-KeyFrame -->
<param name="Vis/CorType" type="string" value="0"/> <!-- 0=features matching 1=Optical Flow -->
<param name="Vis/EstimationType" type="string" value="0"/> <!-- 0=3D->3D, 1=3D->2D (PnP) -->
<param name="Odom/Strategy" type="string" value="0"/> <!-- 0=Frame-to-Map, 1=Frame-to-KeyFrame -->
<param name="Vis/CorType" type="string" value="0"/> <!-- 0=features matching 1=Optical Flow -->
<param name="Vis/EstimationType" type="string" value="0"/> <!-- 0=3D->3D, 1=3D->2D (PnP) -->
<param name="Odom/FillInfoData" type="string" value="$(arg rtabmapviz)"/>
<param name="Vis/UseDepthAsMask" type="string" value="true"/>
<param name="Vis/MaxDepth" type="string" value="4"/>
<param name="Vis/PnPRefineIterations" type="string" value="0"/>
<param name="Odom/ResetCountdown" type="string" value="15"/>
<param name="Vis/CorNNDR" type="string" value="0.6"/>
<param name="Odom/ResetCountdown" type="string" value="15"/>
<param name="Odom/KeyFrameThr" type="string" value="0.5"/>
<param name="odom_frame_id" type="string" value="vis_odom"/>
<param name="frame_id" type="string" value="kinect"/>
<param name="publish_tf" type="bool" value="false"/>
<param name="queue_size" type="int" value="30"/>
<param name="wait_for_transform" type="bool" value="true"/>
<param name="ground_truth_frame_id" type="string" value="world"/>
<param name="ground_truth_frame_id" type="string" value="world"/>
</node>
<!-- rename the child frame of odometry -->
<node name="odom_msg_to_tf" pkg="rtabmap_ros" type="odom_msg_to_tf">
@@ -57,13 +57,11 @@
<param name="subscribe_laserScan" type="bool" value="false"/>
<param name="Rtabmap/StartNewMapOnLoopClosure" type="string" value="true"/>
<param name="Vis/EstimationType" type="string" value="1"/>
<param name="Vis/EstimationType" type="string" value="0"/>
<param name="Vis/MaxDepth" type="string" value="4"/>
<param name="Vis/PnPRefineIterations" type="string" value="0"/>
<param name="RGBD/LoopClosureReextractFeatures" type="string" value="false"/>
<param name="Mem/RawDescriptorsKept" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="6"/>
<param name="Mem/UseDepthAsMask" type="string" value="true"/>
<param name="Kp/DetectorStrategy" type="string" value="0"/>
<param name="frame_id" type="string" value="kinect"/>
<param name="ground_truth_frame_id" type="string" value="world"/>
msg/OdomInfo.msg
+2
View File
@@ -42,6 +42,8 @@ int32[] wordsKeys
KeyPoint[] wordsValues
int32[] wordMatches
int32[] wordInliers
int32[] localMapKeys
Point3f[] localMapValues
Point2f[] refCorners
Point2f[] newCorners
msg/Point3f.msg
+10
View File
@@ -0,0 +1,10 @@
#class cv::Point3f
#{
# float x;
# float y;
# float z;
#}
float32 x
float32 y
float32 z
src/CoreWrapper.cpp
+1 -1
View File
@@ -925,7 +925,7 @@ void CoreWrapper::commonDepthCallback(
if(sensorT.isNull())
{
ROS_WARN("Could not get odometry value for laser scan stamp (%fs). Latest odometry "
"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scanMsg->header.stamp.toSec(), lastPoseStamp_.toSec());
"stamp is %fs. The laser scan pose will not be synchronized with odometry.", scan2dMsg->header.stamp.toSec(), lastPoseStamp_.toSec());
}
else
{
src/MsgConversion.cpp
+45 -3
View File
@@ -295,6 +295,37 @@ void points2fToROS(const std::vector<cv::Point2f> & kpts, std::vector<rtabmap_ro
}
}
cv::Point3f point3fFromROS(const rtabmap_ros::Point3f & msg)
{
return cv::Point3f(msg.x, msg.y, msg.z);
}
void point3fToROS(const cv::Point3f & kpt, rtabmap_ros::Point3f & msg)
{
msg.x = kpt.x;
msg.y = kpt.y;
msg.z = kpt.z;
}
std::vector<cv::Point3f> points3fFromROS(const std::vector<rtabmap_ros::Point3f> & msg)
{
std::vector<cv::Point3f> v(msg.size());
for(unsigned int i=0; i<msg.size(); ++i)
{
v[i] = point3fFromROS(msg[i]);
}
return v;
}
void points3fToROS(const std::vector<cv::Point3f> & kpts, std::vector<rtabmap_ros::Point3f> & msg)
{
msg.resize(kpts.size());
for(unsigned int i=0; i<msg.size(); ++i)
{
point3fToROS(kpts[i], msg[i]);
}
}
rtabmap::CameraModel cameraModelFromROS(
const sensor_msgs::CameraInfo & camInfo,
const rtabmap::Transform & localTransform)
@@ -306,7 +337,9 @@ rtabmap::CameraModel cameraModelFromROS(
model.fy(),
model.cx(),
model.cy(),
localTransform);
localTransform,
0.0,
cv::Size(model.fullResolution().width, model.fullResolution().height));
}
rtabmap::StereoCameraModel stereoCameraModelFromROS(
const sensor_msgs::CameraInfo & leftCamInfo,
@@ -321,7 +354,8 @@ rtabmap::StereoCameraModel stereoCameraModelFromROS(
model.left().cx(),
model.left().cy(),
model.baseline(),
localTransform);
localTransform,
cv::Size(model.left().fullResolution().width, model.left().fullResolution().height));
}
void mapDataFromROS(
@@ -647,6 +681,12 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
info.transform = transformFromGeometryMsg(msg.transform);
info.transformFiltered = transformFromGeometryMsg(msg.transformFiltered);
UASSERT(msg.localMapKeys.size() == msg.localMapValues.size());
for(unsigned int i=0; i<msg.localMapKeys.size(); ++i)
{
info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i])));
}
return info;
}
@@ -664,7 +704,6 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
msg.interval = info.interval;
msg.distanceTravelled = info.distanceTravelled;
msg.type = info.type;
msg.wordsKeys = uKeys(info.words);
@@ -680,6 +719,9 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
transformToGeometryMsg(info.transform, msg.transform);
transformToGeometryMsg(info.transformFiltered, msg.transformFiltered);
msg.localMapKeys = uKeys(info.localMap);
points3fToROS(uValues(info.localMap), msg.localMapValues);
}
}
src/OdometryROS.cpp
+7 -8
View File
@@ -369,15 +369,14 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
//set velocity
if(previousStamp_.isValid())
{
float dt = 1.0f/(stamp - previousStamp_).toSec();
float x,y,z,roll,pitch,yaw;
odometry_->previousTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
odom.twist.twist.linear.x = x*dt;
odom.twist.twist.linear.y = y*dt;
odom.twist.twist.linear.z = z*dt;
odom.twist.twist.angular.x = roll*dt;
odom.twist.twist.angular.y = pitch*dt;
odom.twist.twist.angular.z = yaw*dt;
odometry_->previousVelocityTransform().getTranslationAndEulerAngles(x,y,z,roll,pitch,yaw);
odom.twist.twist.linear.x = x;
odom.twist.twist.linear.y = y;
odom.twist.twist.linear.z = z;
odom.twist.twist.angular.x = roll;
odom.twist.twist.angular.y = pitch;
odom.twist.twist.angular.z = yaw;
}
previousStamp_ = stamp;