12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Transform landmark poses to the tracking frame. (#1076) 

* Transform landmark poses to the tracking frame.

* Address the comment.
master
Alexander Belyaev 2018-10-25 14:43:43 +02:00 committed by GitHub
parent 54f0cfeee4
commit a4f3850912
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 14 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

@ -30,7 +30,7 @@ namespace {
using ::cartographer::transform::Rigid3d;
constexpr double kTrajectoryLineStripMarkerScale = 0.07;
constexpr double kLandmarkMarkerScale = 0.3;
constexpr double kLandmarkMarkerScale = 0.2;
constexpr double kConstraintMarkerScale = 0.025;
::std_msgs::ColorRGBA ToMessage(const cartographer::io::FloatColor& color) {
@ -75,7 +75,7 @@ visualization_msgs::Marker CreateLandmarkMarker(int landmark_index,
visualization_msgs::Marker marker;
marker.ns = "Landmarks";
marker.id = landmark_index;
marker.type = visualization_msgs::Marker::CUBE;
marker.type = visualization_msgs::Marker::SPHERE;
marker.header.stamp = ::ros::Time::now();
marker.header.frame_id = frame_id;
marker.scale.x = kLandmarkMarkerScale;

13
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -100,7 +100,18 @@ void SensorBridge::HandleNavSatFixMessage(
void SensorBridge::HandleLandmarkMessage(
const std::string& sensor_id,
const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
trajectory_builder_->AddSensorData(sensor_id, ToLandmarkData(*msg));
auto landmark_data = ToLandmarkData(*msg);
auto tracking_from_landmark_sensor = tf_bridge_.LookupToTracking(
landmark_data.time, CheckNoLeadingSlash(msg->header.frame_id));
if (tracking_from_landmark_sensor != nullptr) {
for (auto& observation : landmark_data.landmark_observations) {
observation.landmark_to_tracking_transform =
*tracking_from_landmark_sensor *
observation.landmark_to_tracking_transform;
}
}
trajectory_builder_->AddSensorData(sensor_id, landmark_data);
}
std::unique_ptr<carto::sensor::ImuData> SensorBridge::ToImuData(