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_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/src/node.cpp b/src/node.cpp index 686314acbc3e1fa5312fef71a9ac6f832f1a95d1..e7762e89935aee2ac0a6eceeb71b3f9aa68b8e03 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..."); @@ -91,7 +98,22 @@ void WolfRosNode::solve() std::string report = solver_->solve(); 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_) + { + if (solver_->computeCovariances(cov_enum_)) + { + last_cov_stamp_ = ros::Time::now(); + if (solver_->getVerbosity() != SolverManager::ReportVerbosity::QUIET) + ROS_INFO("Covariances computed successfully!"); + } + else if (solver_->getVerbosity() != SolverManager::ReportVerbosity::QUIET) + ROS_WARN("Failed to compute covariances"); + } } void WolfRosNode::visualize() @@ -184,7 +206,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()) diff --git a/src/publisher_pose.cpp b/src/publisher_pose.cpp index 11fc3937928ac16df25970ceb80e7f187a4aac4b..328dd6bc3ce597c10b1d4b0d7b7a42e50d62d922 100644 --- a/src/publisher_pose.cpp +++ b/src/publisher_pose.cpp @@ -15,8 +15,9 @@ 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"); + pose_array_ = _server.getParam<bool>(prefix_ + "/pose_array_msg"); + marker_ = _server.getParam<bool>(prefix_ + "/marker_msg"); + pose_with_cov_ = _server.getParam<bool>(prefix_ + "/pose_with_cov_msg"); if (marker_) { Eigen::Vector4d col = _server.getParam<Eigen::Vector4d>(prefix_ + "/marker_color"); @@ -55,22 +56,30 @@ void PublisherPose::initialize(ros::NodeHandle& nh, const std::string& topic) pub_marker_ = nh.advertise<visualization_msgs::Marker>(topic + "_marker", 1); } + if (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 (not pose_array_ and not marker_ and not pose_with_cov_) + 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 +128,63 @@ void PublisherPose::publishDerived() q = q_frame_ * q; } + // Covariance + Eigen::MatrixXd cov(6,6); + auto KF = problem_->getLastKeyFrame(); + WOLF_INFO("Getting cov of KF ", KF->id()); + 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_) { - 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_) { - 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 (pose_with_cov_ and pose_with_cov_msg_.pose.covariance[0] > 0) + { + pub_pose_with_cov_.publish(pose_with_cov_msg_); + } } bool PublisherPose::listenTf()