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
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
|
google_library(map_writer
|
||||||
USES_GLOG
|
USES_GLOG
|
||||||
USES_ROS
|
USES_ROS
|
||||||
|
@ -87,14 +95,6 @@ google_library(time_conversion
|
||||||
time_conversion.h
|
time_conversion.h
|
||||||
)
|
)
|
||||||
|
|
||||||
google_library(xray
|
|
||||||
USES_CARTOGRAPHER
|
|
||||||
SRCS
|
|
||||||
xray.cc
|
|
||||||
HDRS
|
|
||||||
xray.h
|
|
||||||
)
|
|
||||||
|
|
||||||
google_catkin_test(time_conversion_test
|
google_catkin_test(time_conversion_test
|
||||||
USES_CARTOGRAPHER
|
USES_CARTOGRAPHER
|
||||||
USES_ROS
|
USES_ROS
|
||||||
|
@ -113,6 +113,7 @@ google_binary(cartographer_node
|
||||||
SRCS
|
SRCS
|
||||||
node_main.cc
|
node_main.cc
|
||||||
DEPENDS
|
DEPENDS
|
||||||
|
assets_writer
|
||||||
map_writer
|
map_writer
|
||||||
msg_conversion
|
msg_conversion
|
||||||
node_options
|
node_options
|
||||||
|
@ -121,7 +122,6 @@ google_binary(cartographer_node
|
||||||
sensor_bridge
|
sensor_bridge
|
||||||
tf_bridge
|
tf_bridge
|
||||||
time_conversion
|
time_conversion
|
||||||
xray
|
|
||||||
)
|
)
|
||||||
|
|
||||||
install(TARGETS cartographer_node
|
install(TARGETS cartographer_node
|
||||||
|
|
|
@ -14,9 +14,11 @@
|
||||||
* limitations under the License.
|
* 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/null_points_processor.h"
|
||||||
|
#include "cartographer/io/ply_writing_points_processor.h"
|
||||||
#include "cartographer/io/points_processor.h"
|
#include "cartographer/io/points_processor.h"
|
||||||
#include "cartographer/io/xray_points_processor.h"
|
#include "cartographer/io/xray_points_processor.h"
|
||||||
|
|
||||||
|
@ -24,9 +26,9 @@ namespace cartographer_ros {
|
||||||
|
|
||||||
namespace carto = ::cartographer;
|
namespace carto = ::cartographer;
|
||||||
|
|
||||||
void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>&
|
void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
|
||||||
trajectory_nodes,
|
trajectory_nodes,
|
||||||
const double voxel_size, const std::string& stem) {
|
const double voxel_size, const std::string& stem) {
|
||||||
carto::io::NullPointsProcessor null_points_processor;
|
carto::io::NullPointsProcessor null_points_processor;
|
||||||
carto::io::XRayPointsProcessor xy_xray_points_processor(
|
carto::io::XRayPointsProcessor xy_xray_points_processor(
|
||||||
voxel_size, carto::transform::Rigid3f::Rotation(
|
voxel_size, carto::transform::Rigid3f::Rotation(
|
||||||
|
@ -40,18 +42,25 @@ void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>&
|
||||||
voxel_size, carto::transform::Rigid3f::Rotation(
|
voxel_size, carto::transform::Rigid3f::Rotation(
|
||||||
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
|
Eigen::AngleAxisf(-M_PI / 2.f, Eigen::Vector3f::UnitZ())),
|
||||||
stem + "_xray_xz.png", &yz_xray_points_processor);
|
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) {
|
for (const auto& node : trajectory_nodes) {
|
||||||
const carto::sensor::LaserFan laser_fan = carto::sensor::TransformLaserFan(
|
const carto::sensor::LaserFan laser_fan = carto::sensor::TransformLaserFan(
|
||||||
carto::sensor::Decompress(node.constant_data->laser_fan_3d),
|
carto::sensor::Decompress(node.constant_data->laser_fan_3d),
|
||||||
node.pose.cast<float>());
|
node.pose.cast<float>());
|
||||||
|
|
||||||
carto::io::PointsBatch points_batch;
|
auto points_batch = carto::common::make_unique<carto::io::PointsBatch>();
|
||||||
points_batch.origin = laser_fan.origin;
|
points_batch->origin = laser_fan.origin;
|
||||||
points_batch.points = laser_fan.returns;
|
points_batch->points = laser_fan.returns;
|
||||||
xz_xray_points_processor.Process(points_batch);
|
for (const uint8 reflectivity :
|
||||||
|
node.constant_data->laser_fan_3d.reflectivities) {
|
||||||
|
points_batch->colors.push_back(
|
||||||
|
carto::io::Color{{reflectivity, reflectivity, reflectivity}});
|
||||||
|
}
|
||||||
|
ply_writing_points_processor.Process(std::move(points_batch));
|
||||||
}
|
}
|
||||||
xz_xray_points_processor.Flush();
|
ply_writing_points_processor.Flush();
|
||||||
}
|
}
|
||||||
|
|
||||||
} // namespace cartographer_ros
|
} // namespace cartographer_ros
|
|
@ -14,8 +14,8 @@
|
||||||
* limitations under the License.
|
* limitations under the License.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
#ifndef CARTOGRAPHER_ROS_XRAY_H_
|
#ifndef CARTOGRAPHER_ROS_ASSETS_WRITER_H_
|
||||||
#define CARTOGRAPHER_ROS_XRAY_H_
|
#define CARTOGRAPHER_ROS_ASSETS_WRITER_H_
|
||||||
|
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
@ -24,12 +24,12 @@
|
||||||
|
|
||||||
namespace cartographer_ros {
|
namespace cartographer_ros {
|
||||||
|
|
||||||
// Writes X-ray images from the 'trajectory_nodes'. The filenames will all start
|
// Writes X-ray images and PLY files from the 'trajectory_nodes'. The filenames
|
||||||
// with 'stem'.
|
// will all start with 'stem'.
|
||||||
void WriteXRayImages(const std::vector<::cartographer::mapping::TrajectoryNode>&
|
void WriteAssets(const std::vector<::cartographer::mapping::TrajectoryNode>&
|
||||||
trajectory_nodes,
|
trajectory_nodes,
|
||||||
double voxel_size, const std::string& stem);
|
double voxel_size, const std::string& stem);
|
||||||
|
|
||||||
} // namespace cartographer_ros
|
} // 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/sensor/proto/sensor.pb.h"
|
||||||
#include "cartographer/transform/rigid_transform.h"
|
#include "cartographer/transform/rigid_transform.h"
|
||||||
#include "cartographer/transform/transform.h"
|
#include "cartographer/transform/transform.h"
|
||||||
|
#include "cartographer_ros/assets_writer.h"
|
||||||
#include "cartographer_ros/map_writer.h"
|
#include "cartographer_ros/map_writer.h"
|
||||||
#include "cartographer_ros/msg_conversion.h"
|
#include "cartographer_ros/msg_conversion.h"
|
||||||
#include "cartographer_ros/node_options.h"
|
#include "cartographer_ros/node_options.h"
|
||||||
|
@ -55,7 +56,6 @@
|
||||||
#include "cartographer_ros/sensor_bridge.h"
|
#include "cartographer_ros/sensor_bridge.h"
|
||||||
#include "cartographer_ros/tf_bridge.h"
|
#include "cartographer_ros/tf_bridge.h"
|
||||||
#include "cartographer_ros/time_conversion.h"
|
#include "cartographer_ros/time_conversion.h"
|
||||||
#include "cartographer_ros/xray.h"
|
|
||||||
#include "cartographer_ros_msgs/FinishTrajectory.h"
|
#include "cartographer_ros_msgs/FinishTrajectory.h"
|
||||||
#include "cartographer_ros_msgs/SubmapEntry.h"
|
#include "cartographer_ros_msgs/SubmapEntry.h"
|
||||||
#include "cartographer_ros_msgs/SubmapList.h"
|
#include "cartographer_ros_msgs/SubmapList.h"
|
||||||
|
@ -378,11 +378,11 @@ bool Node::HandleFinishTrajectory(
|
||||||
}
|
}
|
||||||
|
|
||||||
if (options_.map_builder_options.use_trajectory_builder_3d()) {
|
if (options_.map_builder_options.use_trajectory_builder_3d()) {
|
||||||
WriteXRayImages(trajectory_nodes,
|
WriteAssets(trajectory_nodes,
|
||||||
options_.map_builder_options.trajectory_builder_3d_options()
|
options_.map_builder_options.trajectory_builder_3d_options()
|
||||||
.submaps_options()
|
.submaps_options()
|
||||||
.high_resolution(),
|
.high_resolution(),
|
||||||
request.stem);
|
request.stem);
|
||||||
}
|
}
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue