diff --git a/include/node.h b/include/node.h
index 4e599aa5612080129cb5ae1afaeaf511a5e65dd0..cf719c85d4050abf9301c242e5c514ad16333b37 100644
--- a/include/node.h
+++ b/include/node.h
@@ -64,6 +64,10 @@ class WolfRosNode
protected:
// solver
SolverManagerPtr solver_;
+ bool compute_cov_;
+ SolverManager::CovarianceBlocksToBeComputed cov_enum_;
+ ros::Time last_cov_stamp_;
+ double cov_period_;
// transforms
tf::TransformBroadcaster tfb_;
diff --git a/include/publisher.h b/include/publisher.h
index d962d61abe3c4f0f43fd0230a81863540cbb2418..778becdb237ed933f6592a02d3d4fdfefd49fd04 100644
--- a/include/publisher.h
+++ b/include/publisher.h
@@ -48,7 +48,8 @@ class Publisher
const ParamsServer& _server,
const ProblemPtr _problem) :
problem_(_problem),
- last_publish_time_(ros::Time(0)),
+ first_publish_time_(ros::Time(0)),
+ last_n_period_(0),
prefix_("ROS publisher/" + _unique_name)
{
period_ = _server.getParam<double>(prefix_ + "/period");
@@ -72,20 +73,30 @@ class Publisher
ProblemPtr problem_;
ros::Publisher publisher_;
double period_;
- ros::Time last_publish_time_;
+ ros::Time first_publish_time_;
+ long unsigned int last_n_period_;
std::string prefix_;
std::string topic_;
};
inline void Publisher::publish()
{
- last_publish_time_ = ros::Time::now();
+ if (last_n_period_ == 0)
+ first_publish_time_ = ros::Time::now();
+
publishDerived();
}
inline bool Publisher::ready()
{
- return (ros::Time::now() - last_publish_time_).toSec() > period_;
+ long unsigned int n_period = (long unsigned int)((ros::Time::now() - first_publish_time_).toSec() / period_);
+ if (n_period > last_n_period_)
+ {
+ last_n_period_ = n_period;
+ return true;
+ }
+
+ return false;
}
inline std::string Publisher::getTopic() const
diff --git a/include/publisher_pose.h b/include/publisher_pose.h
index dff4bac7531ea8fccf2c580fae43532f03aab046..890b6dd86a9f9153eee8730af897468660e57bc2 100644
--- a/include/publisher_pose.h
+++ b/include/publisher_pose.h
@@ -13,6 +13,7 @@
**************************/
#include <ros/ros.h>
#include <geometry_msgs/PoseArray.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <visualization_msgs/Marker.h>
#include <tf/transform_listener.h>
@@ -21,16 +22,17 @@ namespace wolf
class PublisherPose: public Publisher
{
- bool pose_array_, marker_;
+ bool pose_array_, marker_, pose_with_cov_;
geometry_msgs::PoseArray pose_array_msg_;
visualization_msgs::Marker marker_msg_;
+ geometry_msgs::PoseWithCovarianceStamped pose_with_cov_msg_;
std_msgs::ColorRGBA marker_color_;
bool extrinsics_;
SensorBasePtr sensor_;
std::string frame_id_, map_frame_id_;
- ros::Publisher pub_pose_array_, pub_marker_;
+ ros::Publisher pub_pose_array_, pub_marker_, pub_pose_with_cov_;
public:
PublisherPose(const std::string& _unique_name,
@@ -44,7 +46,7 @@ class PublisherPose: public Publisher
void publishDerived() override;
- void publishPose(const geometry_msgs::Pose pose, const ros::Time& stamp);
+ void publishPose();
protected:
diff --git a/include/visualizer.h b/include/visualizer.h
index 988da90a5400cd9e06b443a8b5aaf012cae22584..21b89f94d9082a91d27dded1c75a812a9554938e 100644
--- a/include/visualizer.h
+++ b/include/visualizer.h
@@ -66,7 +66,7 @@ class Visualizer
// Options
std::string map_frame_id_;
- bool viz_factors_, viz_landmarks_, viz_trajectory_, viz_overlapped_factors_;
+ bool viz_overlapped_factors_;
double viz_scale_, factors_width_, factors_absolute_height_, landmark_text_z_offset_, landmark_width_, landmark_length_, frame_width_, frame_length_;
std_msgs::ColorRGBA frame_color_, factor_abs_color_, factor_motion_color_, factor_loop_color_, factor_lmk_color_, factor_geom_color_;
diff --git a/src/node.cpp b/src/node.cpp
index 34a456b4d212edad06f786ca95ee8b164d1add65..c08711d2e3533e68b020b9ef5bdba093b29d4095 100644
--- a/src/node.cpp
+++ b/src/node.cpp
@@ -39,6 +39,13 @@ WolfRosNode::WolfRosNode()
// SOLVER
ROS_INFO("Creating solver...");
solver_ = FactorySolver::create("SolverCeres", problem_ptr_, server);
+ // covariance
+ compute_cov_ = server.getParam<bool>("solver/compute_cov");
+ if (compute_cov_)
+ {
+ cov_enum_ = (SolverManager::CovarianceBlocksToBeComputed)server.getParam<int>("solver/cov_enum");
+ cov_period_ = server.getParam<double>("solver/cov_period");
+ }
// ROS SUBSCRIBERS
ROS_INFO("Creating subscribers...");
@@ -82,9 +89,26 @@ void WolfRosNode::solve()
ROS_INFO("================ solve ==================");
std::string report = solver_->solve();
- // if (!report.empty()){
- // std::cout << report << std::endl;
- // }
+
+ if (!report.empty())
+ {
+ std::cout << report << std::endl;
+ //problem_ptr_->print(4,1,1,1);
+ }
+
+ if (compute_cov_ and (ros::Time::now() - last_cov_stamp_).toSec() > cov_period_)
+ {
+ auto start = std::chrono::high_resolution_clock::now();
+ if (solver_->computeCovariances(cov_enum_))
+ {
+ auto duration = std::chrono::duration_cast<std::chrono::microseconds>(std::chrono::high_resolution_clock::now() - start);
+ last_cov_stamp_ = ros::Time::now();
+ if (solver_->getVerbosity() != SolverManager::ReportVerbosity::QUIET)
+ ROS_INFO("Covariances computed successfully! It took %li microseconds", duration.count());
+ }
+ else if (solver_->getVerbosity() != SolverManager::ReportVerbosity::QUIET)
+ ROS_WARN("Failed to compute covariances");
+ }
}
void WolfRosNode::visualize()
@@ -94,7 +118,7 @@ void WolfRosNode::visualize()
viz_->visualize(problem_ptr_);
auto stop = std::chrono::high_resolution_clock::now();
auto duration = std::chrono::duration_cast<std::chrono::microseconds>(stop - start);
- std::cout << "Visualize took " << duration.count() << " microseconds" << std::endl;
+ //std::cout << "Visualize took " << duration.count() << " microseconds" << std::endl;
}
bool WolfRosNode::updateTf()
@@ -177,7 +201,7 @@ void WolfRosNode::broadcastTf()
void WolfRosNode::solveLoop()
{
- ros::Rate solverRate(1/solver_->getPeriod());
+ ros::Rate solverRate(1/(solver_->getPeriod()+1e-9)); // 1ns added to allow pausing if rosbag paused
WOLF_DEBUG("Started solver loop");
while (ros::ok())
@@ -208,6 +232,12 @@ int main(int argc, char **argv)
// Solver thread
std::thread solver_thread(&WolfRosNode::solveLoop, &wolf_node);
+ // set priority
+ struct sched_param Priority_Param; //struct to set priority
+ int priority = 99;
+ Priority_Param.sched_priority = priority;
+ int policy=SCHED_FIFO;
+ pthread_setschedparam(solver_thread.native_handle(), SCHED_FIFO, &Priority_Param);
while (ros::ok())
{
@@ -219,7 +249,7 @@ int main(int argc, char **argv)
auto start3 = std::chrono::high_resolution_clock::now();
if ((ros::Time::now() - last_viz_time).toSec() >= wolf_node.viz_period_)
{
- std::cout << "Last Viz since/viz_period_ " << (ros::Time::now() - last_viz_time).toSec() << " / " << wolf_node.viz_period_ << std::endl;
+ //std::cout << "Last Viz since/viz_period_ " << (ros::Time::now() - last_viz_time).toSec() << " / " << wolf_node.viz_period_ << std::endl;
wolf_node.visualize();
last_viz_time = ros::Time::now();
}
diff --git a/src/publisher_pose.cpp b/src/publisher_pose.cpp
index 11fc3937928ac16df25970ceb80e7f187a4aac4b..7d4a6d4695df29a2f0e9459c2a7a1f6b6555dac4 100644
--- a/src/publisher_pose.cpp
+++ b/src/publisher_pose.cpp
@@ -15,16 +15,23 @@ PublisherPose::PublisherPose(const std::string& _unique_name,
const ProblemPtr _problem) :
Publisher(_unique_name, _server, _problem)
{
- pose_array_ = _server.getParam<bool>(prefix_ + "/pose_array_msg");
- marker_ = _server.getParam<bool>(prefix_ + "/marker_msg");
- if (marker_)
- {
+ try{
+ std::cout << "PublisherPose: taking user defined marker color...\n";
Eigen::Vector4d col = _server.getParam<Eigen::Vector4d>(prefix_ + "/marker_color");
marker_color_.r = col(0);
marker_color_.g = col(1);
marker_color_.b = col(2);
marker_color_.a = col(3);
}
+ catch(...)
+ {
+ std::cout << "PublisherPose: using default marker color: RED\n";
+ marker_color_.r = 1;
+ marker_color_.g = 0;
+ marker_color_.b = 0;
+ marker_color_.a = 1;
+ }
+
extrinsics_ = _server.getParam<bool>(prefix_ + "/extrinsics");
if (extrinsics_)
sensor_ = _problem->getSensor(_server.getParam<std::string>(prefix_ + "/sensor"));
@@ -37,40 +44,47 @@ void PublisherPose::initialize(ros::NodeHandle& nh, const std::string& topic)
nh.param<std::string>("map_frame_id", map_frame_id_, "map");
// initialize msg and publisher
- if (pose_array_)
- {
- pose_array_msg_.header.frame_id = frame_id_;
- pub_pose_array_ = nh.advertise<geometry_msgs::PoseArray>(topic + "_pose_array", 1);
- }
- if (marker_)
- {
- marker_msg_.header.frame_id = frame_id_;
- marker_msg_.type = visualization_msgs::Marker::LINE_STRIP;
- marker_msg_.action = visualization_msgs::Marker::ADD;
- marker_msg_.ns = "trajectory";
- marker_msg_.scale.x = 0.1;
- marker_msg_.color = marker_color_;
- marker_msg_.pose.orientation = tf::createQuaternionMsgFromYaw(0);
-
- pub_marker_ = nh.advertise<visualization_msgs::Marker>(topic + "_marker", 1);
- }
+ // POSE ARRAY
+ pose_array_msg_.header.frame_id = frame_id_;
+
+ pub_pose_array_ = nh.advertise<geometry_msgs::PoseArray>(topic + "_pose_array", 1);
+
+ // MARKER
+ marker_msg_.header.frame_id = frame_id_;
+ marker_msg_.type = visualization_msgs::Marker::LINE_STRIP;
+ marker_msg_.action = visualization_msgs::Marker::ADD;
+ marker_msg_.ns = "trajectory";
+ marker_msg_.scale.x = 0.1;
+ marker_msg_.color = marker_color_;
+ marker_msg_.pose.orientation = tf::createQuaternionMsgFromYaw(0);
+
+ pub_marker_ = nh.advertise<visualization_msgs::Marker>(topic + "_marker", 1);
+
+ // POSE WITH COV
+ pose_with_cov_msg_.header.frame_id = frame_id_;
+
+ pub_pose_with_cov_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>(topic + "_pose_with_cov", 1);
}
void PublisherPose::publishDerived()
{
+ if (pub_pose_array_.getNumSubscribers() == 0 and
+ pub_marker_.getNumSubscribers() == 0 and
+ pub_pose_with_cov_.getNumSubscribers() == 0 )
+ return;
+
VectorComposite current_state = problem_->getState("PO");
TimeStamp loc_ts = problem_->getTimeStamp();
// state not ready
if (current_state.count('P') == 0 or
current_state.count('O') == 0 or
- loc_ts == TimeStamp(0))
+ not loc_ts.ok())
{
return;
}
-
// fill vector and quaternion
Eigen::Vector3d p = Eigen::Vector3d::Zero();
Eigen::Quaterniond q;
@@ -119,36 +133,62 @@ void PublisherPose::publishDerived()
q = q_frame_ * q;
}
+ // Covariance
+ Eigen::MatrixXd cov(6,6);
+ auto KF = problem_->getLastFrame();
+ bool success(true);
+ success = success && problem_->getCovarianceBlock(KF->getP(), KF->getP(), cov, 0, 0);
+ success = success && problem_->getCovarianceBlock(KF->getP(), KF->getO(), cov, 0, 3);
+ success = success && problem_->getCovarianceBlock(KF->getO(), KF->getP(), cov, 3, 0);
+ success = success && problem_->getCovarianceBlock(KF->getO(), KF->getO(), cov, 3, 3);
+
+ if (success)
+ {
+ if (problem_->getDim() == 2)
+ throw std::runtime_error("not implemented");
+ else
+ std::copy(cov.data(), cov.data() + cov.size(), pose_with_cov_msg_.pose.covariance.data());
+ }
+ else
+ {
+ //WOLF_WARN("Last KF covariance could not be recovered, using the previous one");
+ //pose_with_cov_msg_.pose.covariance[0] = -1; // not valid
+ }
+
// Fill Pose msg
- geometry_msgs::Pose pose_msg;
- pose_msg.position.x = p(0);
- pose_msg.position.y = p(1);
- pose_msg.position.z = p(2);
-
- pose_msg.orientation.x = q.x();
- pose_msg.orientation.y = q.y();
- pose_msg.orientation.z = q.z();
- pose_msg.orientation.w = q.w();
- publishPose(pose_msg, ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds()));
+ pose_with_cov_msg_.header.stamp = ros::Time(loc_ts.getSeconds(), loc_ts.getNanoSeconds());
+ pose_with_cov_msg_.pose.pose.position.x = p(0);
+ pose_with_cov_msg_.pose.pose.position.y = p(1);
+ pose_with_cov_msg_.pose.pose.position.z = p(2);
+
+ pose_with_cov_msg_.pose.pose.orientation.x = q.x();
+ pose_with_cov_msg_.pose.pose.orientation.y = q.y();
+ pose_with_cov_msg_.pose.pose.orientation.z = q.z();
+ pose_with_cov_msg_.pose.pose.orientation.w = q.w();
+ publishPose();
}
-void PublisherPose::publishPose(const geometry_msgs::Pose pose, const ros::Time& stamp)
+void PublisherPose::publishPose()
{
// fill msgs and publish
- if (pose_array_)
+ if (pub_pose_array_.getNumSubscribers() != 0)
{
- pose_array_msg_.header.stamp = stamp;
- pose_array_msg_.poses.push_back(pose);
+ pose_array_msg_.header.stamp = pose_with_cov_msg_.header.stamp;
+ pose_array_msg_.poses.push_back(pose_with_cov_msg_.pose.pose);
pub_pose_array_.publish(pose_array_msg_);
}
- if (marker_)
+ if (pub_marker_.getNumSubscribers() != 0)
{
- marker_msg_.header.stamp = stamp;
- marker_msg_.points.push_back(pose.position);
+ marker_msg_.header.stamp = pose_with_cov_msg_.header.stamp;
+ marker_msg_.points.push_back(pose_with_cov_msg_.pose.pose.position);
pub_marker_.publish(marker_msg_);
}
+ if (pub_pose_with_cov_.getNumSubscribers() != 0)
+ {
+ pub_pose_with_cov_.publish(pose_with_cov_msg_);
+ }
}
bool PublisherPose::listenTf()
diff --git a/src/visualizer.cpp b/src/visualizer.cpp
index c99231cd08d4c1084b6645ca11d4b5bad905be4d..af2aad32b34f231ff5f5f3824e7ff5d0883684a8 100644
--- a/src/visualizer.cpp
+++ b/src/visualizer.cpp
@@ -11,16 +11,8 @@ Visualizer::Visualizer() :
void Visualizer::initialize(ros::NodeHandle& nh)
{
- // init publishers ---------------------------------------------------
- factors_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("factors", 1);
- landmarks_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("landmarks", 1);
- trajectory_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("trajectory", 1);
-
// Load options ---------------------------------------------------
- nh.param<bool>( "viz_factors", viz_factors_, true);
nh.param<bool>( "viz_overlapped_factors", viz_overlapped_factors_, false);
- nh.param<bool>( "viz_landmarks", viz_landmarks_, true);
- nh.param<bool>( "viz_trajectory", viz_trajectory_, true);
// viz parameters
nh.param<std::string>( "map_frame_id", map_frame_id_, "map");
nh.param<double>( "viz_scale", viz_scale_, 1);
@@ -62,6 +54,11 @@ void Visualizer::initialize(ros::NodeHandle& nh)
nh.param<float>( "factor_geom_color_b", factor_geom_color_.b, 1.0);
nh.param<float>( "factor_geom_color_a", factor_geom_color_.a, 1);
+ // init publishers ---------------------------------------------------
+ factors_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("factors", 1);
+ landmarks_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("landmarks", 1);
+ trajectory_publisher_ = nh.advertise<visualization_msgs::MarkerArray>("trajectory", 1);
+
// init markers ---------------------------------------------------
// factor markers message
factor_marker_.type = visualization_msgs::Marker::LINE_LIST;
@@ -119,11 +116,11 @@ void Visualizer::initialize(ros::NodeHandle& nh)
void Visualizer::visualize(const ProblemPtr problem)
{
- if (viz_factors_)
+ if (factors_publisher_.getNumSubscribers() != 0)
publishFactors(problem);
- if (viz_landmarks_)
+ if (landmarks_publisher_.getNumSubscribers() != 0)
publishLandmarks(problem);
- if (viz_trajectory_)
+ if (trajectory_publisher_.getNumSubscribers() != 0)
publishTrajectory(problem);
}