Unverified Commit f690f1de authored by Maurice Fallon's avatar Maurice Fallon Committed by GitHub
Browse files

Merge pull request #28 from ori-drs/mf-process-from-file

process from file
parents 44dd6119 c36d2dfa
......@@ -38,7 +38,11 @@ struct CommandLineConfig
bool failure_prediction_mode;
int reference_update_frequency;
float max_correction_magnitude;
int max_queue_size;
bool verbose;
bool write_input_clouds_to_file;
bool process_input_clouds_from_file;
string process_input_clouds_folder;
};
namespace aicp {
......@@ -63,6 +67,10 @@ public:
ClassificationParams class_params_;
int online_results_line_;
void processFromFile(std::string file_path);
void processCloud(AlignedCloudPtr cloud);
// thread function doing actual work
void operator()();
......
class IsometryWithTime
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
public:
IsometryWithTime(){
pose = Eigen::Isometry3d::Identity();
sec = 0;
nsec = 0;
counter = 0;
}
IsometryWithTime(Eigen::Isometry3d pose_in,
int sec_in,
int nsec_in,
int counter_in){
pose = pose_in;
sec = sec_in;
nsec = nsec_in;
counter = counter_in;
}
~IsometryWithTime(){
}
Eigen::Isometry3d pose;
int sec;
int nsec;
int counter;
};
class PoseFileReader
{
public:
PoseFileReader(){
}
~PoseFileReader(){
}
void readPoseFile(std::string file_name, std::vector< IsometryWithTime > &world_to_body_poses){
// Read the input poses file:
ifstream in( file_name );
vector<vector<double>> fields;
if (in) {
string line;
while (getline(in, line)) {
if (line.at(0) == '#'){
continue;
}
stringstream sep(line);
string field;
fields.push_back(vector<double>());
while (getline(sep, field, ',')) {
fields.back().push_back(stod(field));
}
}
}
for (auto row : fields) {
Eigen::Isometry3d world_to_body = Eigen::Isometry3d::Identity();
world_to_body.translation() << row[3],row[4],row[5];
world_to_body.rotate( Eigen::Quaterniond(row[9],row[6],row[7],row[8]) );
int counter = int(row[0]);
int sec = int(row[1]);
int nsec = int(row[2]);
IsometryWithTime world_to_body_pose = IsometryWithTime(world_to_body, sec, nsec, counter);
world_to_body_poses.push_back(world_to_body_pose);
}
}
};
\ No newline at end of file
......@@ -11,7 +11,7 @@ class TimingUtils{
~TimingUtils(){}
static void tic();
static void toc();
static void toc(std::string message = "");
static std::string currentDateTime();
static void sleepSeconds(clock_t sec);
......
......@@ -5,6 +5,7 @@
#include "aicp_utils/timing.hpp"
#include "aicp_utils/common.hpp"
#include "aicp_utils/poseFileReader.hpp"
namespace aicp {
......@@ -224,7 +225,9 @@ void App::runAicpPipeline(pcl::PointCloud<pcl::PointXYZ>::Ptr& reference_prefilt
= Overlap =
===========================*/
TimingUtils::tic();
computeOverlap(reference_prefiltered, reading_prefiltered, reference_pose, reading_pose);
TimingUtils::toc("computeOverlap");
/*=================================
= Alignment Risk =
......@@ -233,33 +236,51 @@ void App::runAicpPipeline(pcl::PointCloud<pcl::PointXYZ>::Ptr& reference_prefilt
if (cl_cfg_.failure_prediction_mode)
computeAlignmentRisk(reference_prefiltered, reading_prefiltered, reference_pose, reading_pose);
TimingUtils::tic();
/*================================
= Registration =
================================*/
if(!cl_cfg_.failure_prediction_mode || // if alignment risk disabled
risk_prediction_(0,0) <= class_params_.svm.threshold) // or below threshold
computeRegistration(*reference_prefiltered, *reading_prefiltered, T);
TimingUtils::toc("computeRegistration");
}
void App::operator()() {
running_ = true;
while (running_) {
std::unique_lock<std::mutex> lock(worker_mutex_);
// Wait for notification from planarLidarHandler
worker_condition_.wait_for(lock, std::chrono::milliseconds(1000));
// Copy current workload from cloud queue to work queue
std::list<AlignedCloudPtr> work_queue;
{
std::unique_lock<std::mutex> lock(data_mutex_);
while (!cloud_queue_.empty()) {
work_queue.push_back(cloud_queue_.front());
cloud_queue_.pop_front();
}
void App::processFromFile(std::string file_path){
std::cout << "starting processFromFile\n";
// Read all poses csv file:
std::stringstream ss;
ss << file_path << "/aicp_input_poses.csv";
std::vector< IsometryWithTime > world_to_body_poses;
PoseFileReader reader;
reader.readPoseFile(ss.str(), world_to_body_poses);
// Process each cloud successively
for (auto pose_with_time : world_to_body_poses) {
std::stringstream ss2;
ss2 << file_path << "/cloud_" << pose_with_time.counter << "_" << pose_with_time.sec << "_" << pose_with_time.nsec << ".pcd";
std::cout << ss2.str() << "\n";
int64_t utime = pose_with_time.sec*1E6 + pose_with_time.nsec;
pcl::PointCloud<pcl::PointXYZ>::Ptr accumulated_cloud (new pcl::PointCloud<pcl::PointXYZ>);
if (pcl::io::loadPCDFile<pcl::PointXYZ> (ss2.str(), *accumulated_cloud) == -1){
std::cout << "Couldn't read file " << ss2.str() << "\n";
return;
}
// Process workload
for (auto cloud : work_queue) {
AlignedCloudPtr current_cloud (new AlignedCloud(utime,
accumulated_cloud,
pose_with_time.pose));
processCloud(current_cloud);
}
}
void App::processCloud(AlignedCloudPtr cloud){
// First point cloud (becomes first reference)
if(!cl_cfg_.localize_against_prior_map &&
......@@ -294,8 +315,9 @@ void App::operator()() {
first_cloud_initialized_ = true;
}
else {
TimingUtils::tic();
TimingUtils::tic();
TimingUtils::tic();
if (cl_cfg_.verbose)
{
// Publish original reading cloud
......@@ -326,6 +348,7 @@ void App::operator()() {
filtered_read << "/reading_prefiltered.pcd";
pcd_writer_.write<pcl::PointXYZ> (filtered_read.str (), *read_prefiltered, false);
}
TimingUtils::toc("setAndFilterReading");
/*=====================================
= AICP Registration =
......@@ -335,6 +358,7 @@ void App::operator()() {
runAicpPipeline(ref_prefiltered, read_prefiltered, ref_pose, read_pose, correction);
TimingUtils::tic();
if(!cl_cfg_.failure_prediction_mode || // if alignment risk disabled
risk_prediction_(0,0) <= class_params_.svm.threshold) // or below threshold
{
......@@ -345,7 +369,7 @@ void App::operator()() {
aligned_clouds_graph_->getNbClouds() != 0)
{
cout << "[Main] -----> WRONG ALIGNMENT: DROPPED POINT CLOUD" << endl;
break;
return;
}
pcl::transformPointCloud (*read_prefiltered, *output, correction);
......@@ -385,12 +409,14 @@ void App::operator()() {
updates_counter_ ++;
cout << "[Main] -----> ALIGNMENT RISK REFERENCE UPDATE" << endl;
}
TimingUtils::toc("updateReference");
initialT_ = correction * initialT_;
/*======================================
= Save and Visualize =
======================================*/
TimingUtils::tic();
// Store chain of corrections for publishing
total_correction_ = fromMatrix4fToIsometry3d(initialT_);
......@@ -477,8 +503,8 @@ void App::operator()() {
aligned_read << "/reading_aligned.pcd";
pcd_writer_.write<pcl::PointXYZ> (aligned_read.str (), *aligned_clouds_graph_->getLastCloud()->getCloud(), false);
}
TimingUtils::toc();
TimingUtils::toc("postProcessing");
TimingUtils::toc("fullLoop");
cout << "============================" << endl
<< "[Main] Summary:" << endl
......@@ -493,6 +519,32 @@ void App::operator()() {
cout << "Updates: " << updates_counter_ << endl;
}
cout << "--------------------------------------------------------------------------------------" << endl;
}
void App::operator()() {
running_ = true;
while (running_) {
std::unique_lock<std::mutex> lock(worker_mutex_);
// Wait for notification from planarLidarHandler
worker_condition_.wait_for(lock, std::chrono::milliseconds(1000));
// Copy current workload from cloud queue to work queue
std::list<AlignedCloudPtr> work_queue;
{
std::unique_lock<std::mutex> lock(data_mutex_);
while (!cloud_queue_.empty()) {
work_queue.push_back(cloud_queue_.front());
cloud_queue_.pop_front();
}
}
// Process workload
for (auto cloud : work_queue) {
processCloud(cloud);
}
}
}
......
......@@ -7,10 +7,10 @@ void TimingUtils::tic() {
tictoc_stack.push(clock());
}
void TimingUtils::toc() {
void TimingUtils::toc(std::string message) {
std::cout << "Time elapsed: "
<< ((double)(clock() - tictoc_stack.top())) / CLOCKS_PER_SEC
<< " sec"
<< " sec " << message
<< std::endl;
tictoc_stack.pop();
}
......
......@@ -24,11 +24,17 @@ public:
const ClassificationParams& class_params);
inline ~AppROS() {
if (input_poses_file_.is_open()) {
input_poses_file_.close();
}
delete accu_;
delete vis_ros_;
delete talk_ros_;
}
void writeCloudToFile(AlignedCloudPtr cloud);
// Subscriber callabacks
void velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr& laser_msg_in);
void robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr& pose_msg_in);
......@@ -45,6 +51,9 @@ public:
private:
ros::NodeHandle& nh_;
std::ofstream input_poses_file_;
int input_clouds_counter_;
Eigen::Isometry3d world_to_body_;
Eigen::Isometry3d world_to_body_previous_;
tf::Pose temp_tf_pose_;
......
......@@ -17,7 +17,7 @@ struct VelodyneAccumulatorConfig
int batch_size = 10;
double max_range = 30;
double min_range = 0.5;
std::string lidar_topic = "/velodyne/point_cloud_filtered";
std::string lidar_topic = "/point_cloud_filter/velodyne/point_cloud_filtered";
std::string inertial_frame = "/odom";
};
......@@ -35,7 +35,7 @@ public:
uint16_t getCounter() const;
bool getFinished() const;
uint64_t getFinishedTime() const;
int64_t getFinishedTime() const;
const PointCloud& getCloud();
void clearCloud();
......@@ -53,7 +53,7 @@ private:
// implicitly discarding intensities from the clouds
PointCloud point_cloud_;
PointCloud accumulated_point_cloud_;
uint64_t utime_;
int64_t utime_;
ros::NodeHandle& nh_;
ros::Subscriber lidar_sub_;
......
......@@ -20,6 +20,11 @@
<arg name="saveProbs" default="$(find aicp_core)/data/classification/probs_opencv3.txt"/>
<arg name="modelLocation" default="$(find aicp_core)/data/models/svm_opencv3.xml" />
<arg name="write_input_clouds_to_file" default="false"/>
<arg name="process_input_clouds_from_file" default="false"/>
<arg name="process_input_clouds_folder" default="/tmp/aicp_data/"/>
<!-- Run AICP (command line configuration) -->
<node pkg="aicp_ros" type="aicp_ros_node" name="aicp_ros_node" output="screen">
......@@ -56,11 +61,21 @@
<param name="reference_update_frequency" value="5" />
<!-- Max allowed correction magnitude (probably failed alignment otherwise) -->
<param name="max_correction_magnitude" value="1.0" /> <!-- 1.5 to generate Ground Truth (David IROS19) -->
<!-- Max length of the queue of accumulated point clouds. was 100 previously -->
<param name="max_queue_size" value="1" />
<!-- 3D point cloud characteristics -->
<param name="batch_size" value="7" />
<!-- Visualize and store -->
<param name="verbose" value="false" />
<!-- Write out incoming data to file -->
<param name="write_input_clouds_to_file" value="$(arg write_input_clouds_to_file)" />
<!-- Read incoming data from file -->
<param name="process_input_clouds_from_file" value="$(arg process_input_clouds_from_file)" />
<!-- Read incoming data from file -->
<param name="process_input_clouds_folder" value="$(arg process_input_clouds_folder)" />
</node>
</launch>
......@@ -8,10 +8,19 @@
<arg name="localize_against_prior_map" default="false"/> <!-- ! should always be false in this launch file ! -->
<arg name="localize_against_built_map" default="false"/> <!-- implicit loop closure when revisit known place -->
<!-- options for post processing -->
<arg name="write_input_clouds_to_file" default="false"/>
<arg name="process_input_clouds_from_file" default="false"/>
<arg name="process_input_clouds_folder" default="/tmp/aicp_data"/>
<include file="$(find aicp_ros)/launch/aicp.launch">
<arg name="load_map_from_file" value="$(arg load_map_from_file)"/>
<arg name="localize_against_prior_map" value="$(arg localize_against_prior_map)"/>
<arg name="localize_against_built_map" value="$(arg localize_against_built_map)"/>
<arg name="write_input_clouds_to_file" value="$(arg write_input_clouds_to_file)"/>
<arg name="process_input_clouds_from_file" value="$(arg process_input_clouds_from_file)"/>
<arg name="process_input_clouds_folder" value="$(arg process_input_clouds_folder)"/>
</include>
<group if="$(arg load_map_from_file)">
......
......@@ -27,16 +27,20 @@ int main(int argc, char** argv){
cl_cfg.reference_update_frequency = 5;
cl_cfg.max_correction_magnitude = 0.5; // Max allowed correction magnitude
// (probably failed alignment otherwise)
cl_cfg.max_queue_size = 3; // maximum length of the queue of accumulated point clouds. was 100 previously
cl_cfg.pose_body_channel = "/state_estimator/pose_in_odom";
cl_cfg.output_channel = "/aicp/pose_corrected"; // Create new channel...
cl_cfg.verbose = false; // enable visualization for debug
cl_cfg.write_input_clouds_to_file = false; // write the raw incoming point clouds to a folder, for post processing
cl_cfg.process_input_clouds_from_file = false; // process raw incoming point cloud from a folder
cl_cfg.process_input_clouds_folder = "/tmp/aicp_data";
aicp::VelodyneAccumulatorConfig va_cfg;
va_cfg.batch_size = 80; // 240 is about 1 sweep at 5RPM // 80 is about 1 sweep at 15RPM
va_cfg.min_range = 0.50; // 1.85; // remove all the short range points
va_cfg.max_range = 15.0; // we can set up to 30 meters (guaranteed range)
va_cfg.lidar_topic ="/velodyne/point_cloud_filtered";
va_cfg.lidar_topic ="/point_cloud_filter/velodyne/point_cloud_filtered";
va_cfg.inertial_frame = "/odom";
nh.getParam("registration_config_file", cl_cfg.registration_config_file);
......@@ -53,10 +57,15 @@ int main(int argc, char** argv){
nh.getParam("failure_prediction_mode", cl_cfg.failure_prediction_mode);
nh.getParam("reference_update_frequency", cl_cfg.reference_update_frequency);
nh.getParam("max_correction_magnitude", cl_cfg.max_correction_magnitude);
nh.getParam("max_queue_size", cl_cfg.max_queue_size);
nh.getParam("pose_body_channel", cl_cfg.pose_body_channel);
nh.getParam("output_channel", cl_cfg.output_channel);
nh.getParam("verbose", cl_cfg.verbose);
nh.getParam("write_input_clouds_to_file", cl_cfg.write_input_clouds_to_file);
nh.getParam("process_input_clouds_from_file", cl_cfg.process_input_clouds_from_file);
nh.getParam("process_input_clouds_folder", cl_cfg.process_input_clouds_folder);
nh.getParam("batch_size", va_cfg.batch_size);
nh.getParam("min_range", va_cfg.min_range);
......@@ -100,19 +109,25 @@ int main(int argc, char** argv){
overlap_params,
classification_params));
// Subscribers
ros::Subscriber lidar_sub = nh.subscribe(va_cfg.lidar_topic, 100, &aicp::AppROS::velodyneCallBack, app.get());
ros::Subscriber pose_sub = nh.subscribe(cl_cfg.pose_body_channel, 100, &aicp::AppROS::robotPoseCallBack, app.get());
ros::Subscriber marker_sub = nh.subscribe("/interaction_marker/pose", 100, &aicp::AppROS::interactionMarkerCallBack, app.get());
if (!cl_cfg.process_input_clouds_from_file){
// Subscribers
ros::Subscriber lidar_sub = nh.subscribe(va_cfg.lidar_topic, 100, &aicp::AppROS::velodyneCallBack, app.get());
ros::Subscriber pose_sub = nh.subscribe(cl_cfg.pose_body_channel, 100, &aicp::AppROS::robotPoseCallBack, app.get());
ros::Subscriber marker_sub = nh.subscribe("/interaction_marker/pose", 100, &aicp::AppROS::interactionMarkerCallBack, app.get());
// Advertise services (using service published by anybotics icp_tools ui)
ros::ServiceServer load_map_server_ = nh.advertiseService("/icp_tools/load_map_from_file", &aicp::AppROS::loadMapFromFileCallBack, app.get());
ros::ServiceServer go_back_server_ = nh.advertiseService("/aicp/go_back_request", &aicp::AppROS::goBackRequestCallBack, app.get());
// Advertise services (using service published by anybotics icp_tools ui)
ros::ServiceServer load_map_server_ = nh.advertiseService("/icp_tools/load_map_from_file", &aicp::AppROS::loadMapFromFileCallBack, app.get());
ros::ServiceServer go_back_server_ = nh.advertiseService("/aicp/go_back_request", &aicp::AppROS::goBackRequestCallBack, app.get());
ROS_INFO_STREAM("[Aicp] Waiting for input messages...");
ROS_INFO_STREAM("[Aicp] Waiting for input messages...");
app->run();
ros::spin();
app->run();
ros::spin();
}else{
app->processFromFile(cl_cfg.process_input_clouds_folder);
}
return 0;
}
......@@ -49,6 +49,20 @@ AppROS::AppROS(ros::NodeHandle &nh,
alignability_pub_ = nh_.advertise<std_msgs::Float32>("/aicp/alignability",10);
risk_pub_ = nh_.advertise<std_msgs::Float32>("/aicp/alignment_risk",10);
}
// Write the incoming data to file. This should be false when running live
if (cl_cfg_.write_input_clouds_to_file)
{
ROS_WARN_STREAM("[Aicp] Writing input clouds to file. Only do this in post processing");
std::stringstream input_poses_filename;
input_poses_filename << data_directory_path_.str() << "/aicp_input_poses.csv";
input_poses_file_.open (input_poses_filename.str().c_str() );
input_poses_file_ << "# counter, sec, nsec, x, y, z, qx, qy, qz, qw\n";
input_poses_file_.flush();
input_clouds_counter_ = 0;
}
}
void AppROS::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedConstPtr &pose_msg_in)
......@@ -134,6 +148,33 @@ void AppROS::robotPoseCallBack(const geometry_msgs::PoseWithCovarianceStampedCon
pose_initialized_ = true;
}
void AppROS::writeCloudToFile(AlignedCloudPtr cloud){
// Extract the data from AlignedCloud
pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud = cloud->getCloud();
Eigen::Isometry3d pose = cloud->getPriorPose();
Eigen::Quaterniond quat(pose.rotation());
int64_t utime = cloud->getUtime();
int64_t sec = floor(utime * 1E-6);
int64_t nsec = utime - sec* 1E6;
// Write the base-to-odom estimate
std::stringstream ss;
ss << input_clouds_counter_ << ", " << sec << ", " << nsec << ", ";
ss << pose.translation().x() << ", " << pose.translation().y() << ", " << pose.translation().z() << ", ";
ss << quat.x() << ", " << quat.y() << ", " << quat.z() << ", " << quat.w();
input_poses_file_ << ss.str() << "\n";
input_poses_file_.flush();
// Write the point cloud
std::stringstream ss2;
ss2 << data_directory_path_.str() << "/cloud_" << input_clouds_counter_ << "_" << sec << "_" << nsec << ".pcd";
pcd_writer_.write<pcl::PointXYZ> (ss2.str (), *point_cloud, true);
input_clouds_counter_++;
}
void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_msg_in){
if (!pose_initialized_){
ROS_WARN_STREAM("[Aicp] Pose not initialized, waiting for pose prior...");
......@@ -180,7 +221,6 @@ void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_ms
// vis_->publishCloud(accumulated_cloud, 10, "/aicp/accumulated_cloud", accu_->getFinishedTime());
// Push this cloud onto the work queue (mutex safe)
const int max_queue_size = 3;
{
std::unique_lock<std::mutex> lock(data_mutex_);
......@@ -190,16 +230,20 @@ void AppROS::velodyneCallBack(const sensor_msgs::PointCloud2::ConstPtr &laser_ms
world_to_body_));
world_to_body_previous_ = world_to_body_;
if (cl_cfg_.write_input_clouds_to_file)
writeCloudToFile(current_cloud);
// Stack current cloud into queue
cloud_queue_.push_back(current_cloud);
//cout << "[App ROS] cloud_queue_ size: " << cloud_queue_.size() << endl;
if (cloud_queue_.size() > max_queue_size) {
if (cloud_queue_.size() > cl_cfg_.max_queue_size) {
cout << "|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||\n";
cout << "|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||\n";
cout << "[App ROS] WARNING: dropping " <<
(cloud_queue_.size()-max_queue_size) << " clouds." << endl;
(cloud_queue_.size()-cl_cfg_.max_queue_size) << " clouds." << endl;
}
while (cloud_queue_.size() > max_queue_size) {
while (cloud_queue_.size() > cl_cfg_.max_queue_size) {
cloud_queue_.pop_front();
}
}
......
......@@ -91,7 +91,7 @@ bool VelodyneAccumulatorROS::getFinished() const {
return finished_;
}
uint64_t VelodyneAccumulatorROS::getFinishedTime() const{
int64_t VelodyneAccumulatorROS::getFinishedTime() const{
return utime_;
}
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment