Adapt to changes in cartographer and write PLY files on exporting. (#128)
Also use reflectivity in the 'PointsBatch'es created in 'WriteAssets'. In the online case, we throw away reflectivities during SLAMing to save on memory, so there will be none.master
Holger Rapp
GitHub
parent
b63611b2c3
commit
ccbf9ef1be
|
|
@ -1,3 +1,11 @@
|
|||
google_library(assets_writer
|
||||
USES_CARTOGRAPHER
|
||||
SRCS
|
||||
assets_writer.cc
|
||||
HDRS
|
||||
assets_writer.h
|
||||
)
|
||||
|
||||
google_library(map_writer
|
||||
USES_GLOG
|
||||
USES_ROS
|
||||
|
|
@ -87,14 +95,6 @@ google_library(time_conversion
|
|||
time_conversion.h
|
||||
)
|
||||
|
||||
google_library(xray
|
||||
USES_CARTOGRAPHER
|
||||
SRCS
|
||||
xray.cc
|
||||
HDRS
|
||||
xray.h
|
||||
)
|
||||
|
||||
google_catkin_test(time_conversion_test
|
||||
USES_CARTOGRAPHER
|
||||
USES_ROS
|
||||
|
|
@ -113,6 +113,7 @@ google_binary(cartographer_node
|
|||
SRCS
|
||||
node_main.cc
|
||||
DEPENDS
|
||||
assets_writer
|
||||
map_writer
|
||||
msg_conversion
|
||||
node_options
|
||||
|
|
@ -121,7 +122,6 @@ google_binary(cartographer_node
|
|||
sensor_bridge
|
||||
tf_bridge
|
||||
time_conversion
|
||||
xray
|
||||
)
|
||||
|
||||
install(TARGETS cartographer_node
|
||||
|
|
|
|||
|
|
@ -14,9 +14,11 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#include "cartographer_ros/xray.h"
|
||||
#include "cartographer_ros/assets_writer.h"
|
||||
|
||||
#include "cartographer/common/make_unique.h"
|
||||
#include "cartographer/io/null_points_processor.h"
|
||||
#include "cartographer/io/ply_writing_points_processor.h"
|
||||
#include "cartographer/io/points_processor.h"
|
||||
#include "cartographer/io/xray_points_processor.h"
|
||||
|
||||
|
|
@ -24,7 +26,7 @@ namespace cartographer_ros {
|
|||
|
||||
namespace carto = ::cartographer;
|
||||
|
||||
void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>&
|
||||
void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
|
||||
trajectory_nodes,
|
||||
const double voxel_size, const std::string& stem) {
|
||||
carto::io::NullPointsProcessor null_points_processor;
|
||||
|
|
@ -40,18 +42,25 @@ void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>&
|
|||
voxel_size, carto::transform::Rigid3f::Rotation(
|
||||
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
|
||||
stem + "_xray_xz.png", &yz_xray_points_processor);
|
||||
carto::io::PlyWritingPointsProcessor ply_writing_points_processor(
|
||||
stem + ".ply", &xz_xray_points_processor);
|
||||
|
||||
for (const auto& node : trajectory_nodes) {
|
||||
const carto::sensor::LaserFan laser_fan = carto::sensor::TransformLaserFan(
|
||||
carto::sensor::Decompress(node.constant_data->laser_fan_3d),
|
||||
node.pose.cast<float>());
|
||||
|
||||
carto::io::PointsBatch points_batch;
|
||||
points_batch.origin = laser_fan.origin;
|
||||
points_batch.points = laser_fan.returns;
|
||||
xz_xray_points_processor.Process(points_batch);
|
||||
auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
|
||||
points_batch->origin = laser_fan.origin;
|
||||
points_batch->points = laser_fan.returns;
|
||||
for (const uint8 reflectivity :
|
||||
node.constant_data->laser_fan_3d.reflectivities) {
|
||||
points_batch->colors.push_back(
|
||||
carto::io::Color{{reflectivity, reflectivity, reflectivity}});
|
||||
}
|
||||
xz_xray_points_processor.Flush();
|
||||
ply_writing_points_processor.Process(std::move(points_batch));
|
||||
}
|
||||
ply_writing_points_processor.Flush();
|
||||
}
|
||||
|
||||
} // namespace cartographer_ros
|
||||
|
|
@ -14,8 +14,8 @@
|
|||
* limitations under the License.
|
||||
*/
|
||||
|
||||
#ifndef CARTOGRAPHER_ROS_XRAY_H_
|
||||
#define CARTOGRAPHER_ROS_XRAY_H_
|
||||
#ifndef CARTOGRAPHER_ROS_ASSETS_WRITER_H_
|
||||
#define CARTOGRAPHER_ROS_ASSETS_WRITER_H_
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
|
@ -24,12 +24,12 @@
|
|||
|
||||
namespace cartographer_ros {
|
||||
|
||||
// Writes X-ray images from the 'trajectory_nodes'. The filenames will all start
|
||||
// with 'stem'.
|
||||
void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>&
|
||||
// Writes X-ray images and PLY files from the 'trajectory_nodes'. The filenames
|
||||
// will all start with 'stem'.
|
||||
void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
|
||||
trajectory_nodes,
|
||||
double voxel_size, const std::string& stem);
|
||||
|
||||
} // namespace cartographer_ros
|
||||
|
||||
#endif // CARTOGRAPHER_ROS_XRAY_H_
|
||||
#endif // CARTOGRAPHER_ROS_ASSETS_WRITER_H_
|
||||
|
|
@ -47,6 +47,7 @@
|
|||
#include "cartographer/sensor/proto/sensor.pb.h"
|
||||
#include "cartographer/transform/rigid_transform.h"
|
||||
#include "cartographer/transform/transform.h"
|
||||
#include "cartographer_ros/assets_writer.h"
|
||||
#include "cartographer_ros/map_writer.h"
|
||||
#include "cartographer_ros/msg_conversion.h"
|
||||
#include "cartographer_ros/node_options.h"
|
||||
|
|
@ -55,7 +56,6 @@
|
|||
#include "cartographer_ros/sensor_bridge.h"
|
||||
#include "cartographer_ros/tf_bridge.h"
|
||||
#include "cartographer_ros/time_conversion.h"
|
||||
#include "cartographer_ros/xray.h"
|
||||
#include "cartographer_ros_msgs/FinishTrajectory.h"
|
||||
#include "cartographer_ros_msgs/SubmapEntry.h"
|
||||
#include "cartographer_ros_msgs/SubmapList.h"
|
||||
|
|
@ -378,7 +378,7 @@ bool Node::HandleFinishTrajectory(
|
|||
}
|
||||
|
||||
if (options_.map_builder_options.use_trajectory_builder_3d()) {
|
||||
WriteXRayImages(trajectory_nodes,
|
||||
WriteAssets(trajectory_nodes,
|
||||
options_.map_builder_options.trajectory_builder_3d_options()
|
||||
.submaps_options()
|
||||
.high_resolution(),
|
||||
|
|
|
|||
Loading…
Reference in New Issue