Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Parse csv format from header #24

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

Parse csv format from header #24

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 13 additions & 13 deletions include/lidar_align/loader.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,32 +11,32 @@
namespace lidar_align {

class Loader {
public:
public:
struct Config {
int use_n_scans = std::numeric_limits<int>::max();
};

Loader(const Config& config);
Loader(const Config &config);

void parsePointcloudMsg(const sensor_msgs::PointCloud2 msg,
LoaderPointcloud* pointcloud);
LoaderPointcloud *pointcloud);

bool loadPointcloudFromROSBag(const std::string& bag_path,
const Scan::Config& scan_config, Lidar* lidar);
bool loadPointcloudFromROSBag(const std::string &bag_path,
const Scan::Config &scan_config, Lidar *lidar);

bool loadTformFromROSBag(const std::string& bag_path, Odom* odom);
bool loadTformFromROSBag(const std::string &bag_path, Odom *odom);

bool loadTformFromMaplabCSV(const std::string& csv_path, Odom* odom);
bool loadTformFromMaplabCSV(const std::string &csv_path, Odom *odom);

static Config getConfig(ros::NodeHandle* nh);
static Config getConfig(ros::NodeHandle *nh);

private:
static bool getNextCSVTransform(std::istream& str, Timestamp* stamp,
Transform* T);
private:
bool getNextCSVTransform(std::istream &str, Timestamp *stamp, Transform *T);

Config config_;
uint first_tf_column_;
};
} // namespace lidar_align
} // namespace lidar_align

POINT_CLOUD_REGISTER_POINT_STRUCT(
lidar_align::PointAllFields,
Expand All @@ -45,4 +45,4 @@ POINT_CLOUD_REGISTER_POINT_STRUCT(
uint16_t, reflectivity, reflectivity)(uint16_t, intensity,
intensity)(uint8_t, ring, ring))

#endif // LIDAR_ALIGN_ALIGNER_H_
#endif // LIDAR_ALIGN_ALIGNER_H_
75 changes: 50 additions & 25 deletions src/loader.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,19 +7,19 @@

namespace lidar_align {

Loader::Loader(const Config& config) : config_(config) {}
Loader::Loader(const Config &config) : config_(config) {}

Loader::Config Loader::getConfig(ros::NodeHandle* nh) {
Loader::Config Loader::getConfig(ros::NodeHandle *nh) {
Loader::Config config;
nh->param("use_n_scans", config.use_n_scans, config.use_n_scans);
return config;
}

void Loader::parsePointcloudMsg(const sensor_msgs::PointCloud2 msg,
LoaderPointcloud* pointcloud) {
LoaderPointcloud *pointcloud) {
bool has_timing = false;
bool has_intensity = false;
for (const sensor_msgs::PointField& field : msg.fields) {
for (const sensor_msgs::PointField &field : msg.fields) {
if (field.name == "time_offset_us") {
has_timing = true;
} else if (field.name == "intensity") {
Expand All @@ -34,7 +34,7 @@ void Loader::parsePointcloudMsg(const sensor_msgs::PointCloud2 msg,
Pointcloud raw_pointcloud;
pcl::fromROSMsg(msg, raw_pointcloud);

for (const Point& raw_point : raw_pointcloud) {
for (const Point &raw_point : raw_pointcloud) {
PointAllFields point;
point.x = raw_point.x;
point.y = raw_point.y;
Expand All @@ -53,7 +53,7 @@ void Loader::parsePointcloudMsg(const sensor_msgs::PointCloud2 msg,
pcl::PointCloud<pcl::PointXYZ> raw_pointcloud;
pcl::fromROSMsg(msg, raw_pointcloud);

for (const pcl::PointXYZ& raw_point : raw_pointcloud) {
for (const pcl::PointXYZ &raw_point : raw_pointcloud) {
PointAllFields point;
point.x = raw_point.x;
point.y = raw_point.y;
Expand All @@ -70,9 +70,9 @@ void Loader::parsePointcloudMsg(const sensor_msgs::PointCloud2 msg,
}
}

bool Loader::loadPointcloudFromROSBag(const std::string& bag_path,
const Scan::Config& scan_config,
Lidar* lidar) {
bool Loader::loadPointcloudFromROSBag(const std::string &bag_path,
const Scan::Config &scan_config,
Lidar *lidar) {
rosbag::Bag bag;
try {
bag.open(bag_path, rosbag::bagmode::Read);
Expand All @@ -86,7 +86,7 @@ bool Loader::loadPointcloudFromROSBag(const std::string& bag_path,
rosbag::View view(bag, rosbag::TypeQuery(types));

size_t scan_num = 0;
for (const rosbag::MessageInstance& m : view) {
for (const rosbag::MessageInstance &m : view) {
std::cout << " Loading scan: \e[1m" << scan_num++ << "\e[0m from ros bag"
<< '\r' << std::flush;

Expand All @@ -110,7 +110,7 @@ bool Loader::loadPointcloudFromROSBag(const std::string& bag_path,
return true;
}

bool Loader::loadTformFromROSBag(const std::string& bag_path, Odom* odom) {
bool Loader::loadTformFromROSBag(const std::string &bag_path, Odom *odom) {
rosbag::Bag bag;
try {
bag.open(bag_path, rosbag::bagmode::Read);
Expand All @@ -124,7 +124,7 @@ bool Loader::loadTformFromROSBag(const std::string& bag_path, Odom* odom) {
rosbag::View view(bag, rosbag::TypeQuery(types));

size_t tform_num = 0;
for (const rosbag::MessageInstance& m : view) {
for (const rosbag::MessageInstance &m : view) {
std::cout << " Loading transform: \e[1m" << tform_num++
<< "\e[0m from ros bag" << '\r' << std::flush;

Expand Down Expand Up @@ -152,9 +152,34 @@ bool Loader::loadTformFromROSBag(const std::string& bag_path, Odom* odom) {
return true;
}

bool Loader::loadTformFromMaplabCSV(const std::string& csv_path, Odom* odom) {
bool Loader::loadTformFromMaplabCSV(const std::string &csv_path, Odom *odom) {
std::ifstream file(csv_path, std::ifstream::in);

// checking the first line for the file format
std::string header;
std::getline(file, header);
if (!header[0] == '#') {
ROS_ERROR("Could not find header in first line of CSV.");
}
std::stringstream line_stream(header);
std::string cell;
size_t column_num = 0;
bool parsing_success = false;
while (std::getline(line_stream, cell, ',')) {
if (cell == " p_G_Ix [m]") {
first_tf_column_ = column_num;
ROS_INFO("Parsed CSV format from header.");
parsing_success = true;
break;
}
column_num++;
}
if (!header[0] == '#' || !parsing_success) {
ROS_WARN("Could not parse CSV header, assuming first data column is 3 "
"(starting at 0).");
first_tf_column_ = 3;
}

size_t tform_num = 0;
while (file.peek() != EOF) {
std::cout << " Loading transform: \e[1m" << tform_num++
Expand All @@ -172,8 +197,8 @@ bool Loader::loadTformFromMaplabCSV(const std::string& csv_path, Odom* odom) {
}

// lots of potential failure cases not checked
bool Loader::getNextCSVTransform(std::istream& str, Timestamp* stamp,
Transform* T) {
bool Loader::getNextCSVTransform(std::istream &str, Timestamp *stamp,
Transform *T) {
std::string line;
std::getline(str, line);

Expand All @@ -190,18 +215,18 @@ bool Loader::getNextCSVTransform(std::istream& str, Timestamp* stamp,
data.push_back(cell);
}

if (data.size() < 9) {
if (data.size() < first_tf_column_ + 7) {
return false;
}

constexpr size_t TIME = 0;
constexpr size_t X = 2;
constexpr size_t Y = 3;
constexpr size_t Z = 4;
constexpr size_t RW = 5;
constexpr size_t RX = 6;
constexpr size_t RY = 7;
constexpr size_t RZ = 8;
size_t TIME = 0;
size_t X = first_tf_column_;
size_t Y = first_tf_column_ + 1;
size_t Z = first_tf_column_ + 2;
size_t RW = first_tf_column_ + 3;
size_t RX = first_tf_column_ + 4;
size_t RY = first_tf_column_ + 5;
size_t RZ = first_tf_column_ + 6;

*stamp = std::stoll(data[TIME]) / 1000ll;
*T = Transform(Transform::Translation(std::stod(data[X]), std::stod(data[Y]),
Expand All @@ -212,4 +237,4 @@ bool Loader::getNextCSVTransform(std::istream& str, Timestamp* stamp,
return true;
}

} // namespace lidar_align
} // namespace lidar_align