From fac938b580651d3f6e1909c7199e564a5088e396 Mon Sep 17 00:00:00 2001 From: Humphrey Hu Date: Mon, 12 Mar 2018 18:22:19 -0400 Subject: [PATCH 1/2] Added an odometry prior callback function --- manycal/include/manycal/Registrations.h | 2 ++ manycal/src/Registrations.cpp | 35 ++++++++++++++++++++++--- 2 files changed, 33 insertions(+), 4 deletions(-) diff --git a/manycal/include/manycal/Registrations.h b/manycal/include/manycal/Registrations.h index 7954b45..6901cf3 100644 --- a/manycal/include/manycal/Registrations.h +++ b/manycal/include/manycal/Registrations.h @@ -176,6 +176,7 @@ class TargetRegistration : public RegistrationBase ros::Subscriber _detSub; ros::Subscriber _odomSub; + ros::Subscriber _odomPriorSub; ros::Time _lastTime; bool _isOdomInitialized; @@ -195,6 +196,7 @@ class TargetRegistration : public RegistrationBase std::string _outputPath; void OdometryCallback( const nav_msgs::Odometry::ConstPtr& msg ); + void OdometryPriorCallback( const nav_msgs::Odometry::ConstPtr& msg); // TODO Support more message types for velocity integration }; } diff --git a/manycal/src/Registrations.cpp b/manycal/src/Registrations.cpp index 60d4941..2f671d8 100644 --- a/manycal/src/Registrations.cpp +++ b/manycal/src/Registrations.cpp @@ -32,18 +32,34 @@ TargetRegistration::TargetRegistration( const std::string& n, double tLen; unsigned int buffLen; std::string topic; + bool enable_odom_prior; GetParam( ph, "integrator_buffer_len", tLen, 10.0 ); _velocityIntegrator.SetMaxBuffLen( tLen ); GetParam( ph, "odom_buffer_len", buffLen, (unsigned int) 10 ); GetParamRequired( ph, "odom_topic", topic ); GetParam( ph, "odom_cov_offset", _odomOffset, PoseSE3::CovarianceMatrix::Zero() ); + GetParam( ph, "enable_odom_prior", enable_odom_prior, false); + _odomSub = nh.subscribe( topic, - buffLen, - &TargetRegistration::OdometryCallback, - this ); + buffLen, + &TargetRegistration::OdometryCallback, + this ); ROS_INFO_STREAM( "Target: " << _name << " subscribing to odometry on " - << topic ); + << topic ); + if(enable_odom_prior) + { + std::string odom_prior_topic; + GetParamRequired( ph, "odom_prior_topic", odom_prior_topic ); + + _odomPriorSub = nh.subscribe( topic, + buffLen, + &TargetRegistration::OdometryPriorCallback, + this ); + + ROS_INFO_STREAM( "Target: " << _name << " subscribing to odometry prior on " + << odom_prior_topic ); + } _poses = std::make_shared( _graph, _optimizePose ); } @@ -215,6 +231,17 @@ void TargetRegistration::OdometryCallback( const nav_msgs::Odometry::ConstPtr& m _velocityIntegrator.BufferInfo( msg->header.stamp.toSec(), vel, cov ); } +void TargetRegistration::OdometryPriorCallback( const nav_msgs::Odometry::ConstPtr& msg ) +{ + ros::Time time = msg->header.stamp; + PoseSE3 prior = MsgToPose( msg->pose.pose ); + PoseSE3::CovarianceMatrix cov; + ParseMatrix ( msg->pose.covariance, cov ); + + isam::PoseSE3 p = PoseToIsam( prior ); + _poses->CreatePrior( time, p, isam::Covariance( cov ) ); +} + ExtrinsicsRegistration::ExtrinsicsRegistration( TargetRegistration& p, const std::string& n, GraphOptimizer& g, From 7100c36816dccc8f65086f019fcc3bf11fd1136f Mon Sep 17 00:00:00 2001 From: Humphrey Hu Date: Mon, 12 Mar 2018 20:02:11 -0400 Subject: [PATCH 2/2] Merged the odom callbacks into one Removed duplicated subscriber --- manycal/include/manycal/Registrations.h | 3 +- manycal/src/Registrations.cpp | 47 ++++++++++--------------- 2 files changed, 19 insertions(+), 31 deletions(-) diff --git a/manycal/include/manycal/Registrations.h b/manycal/include/manycal/Registrations.h index 6901cf3..de07312 100644 --- a/manycal/include/manycal/Registrations.h +++ b/manycal/include/manycal/Registrations.h @@ -176,9 +176,9 @@ class TargetRegistration : public RegistrationBase ros::Subscriber _detSub; ros::Subscriber _odomSub; - ros::Subscriber _odomPriorSub; ros::Time _lastTime; + bool _enableOdomPrior; bool _isOdomInitialized; VelocityIntegratorSE3 _velocityIntegrator; PoseSE3::CovarianceMatrix _odomOffset; @@ -196,7 +196,6 @@ class TargetRegistration : public RegistrationBase std::string _outputPath; void OdometryCallback( const nav_msgs::Odometry::ConstPtr& msg ); - void OdometryPriorCallback( const nav_msgs::Odometry::ConstPtr& msg); // TODO Support more message types for velocity integration }; } diff --git a/manycal/src/Registrations.cpp b/manycal/src/Registrations.cpp index 2f671d8..9e6722b 100644 --- a/manycal/src/Registrations.cpp +++ b/manycal/src/Registrations.cpp @@ -32,14 +32,13 @@ TargetRegistration::TargetRegistration( const std::string& n, double tLen; unsigned int buffLen; std::string topic; - bool enable_odom_prior; GetParam( ph, "integrator_buffer_len", tLen, 10.0 ); _velocityIntegrator.SetMaxBuffLen( tLen ); GetParam( ph, "odom_buffer_len", buffLen, (unsigned int) 10 ); GetParamRequired( ph, "odom_topic", topic ); GetParam( ph, "odom_cov_offset", _odomOffset, PoseSE3::CovarianceMatrix::Zero() ); - GetParam( ph, "enable_odom_prior", enable_odom_prior, false); + GetParam( ph, "enable_odom_prior", _enableOdomPrior, false); _odomSub = nh.subscribe( topic, buffLen, @@ -47,19 +46,6 @@ TargetRegistration::TargetRegistration( const std::string& n, this ); ROS_INFO_STREAM( "Target: " << _name << " subscribing to odometry on " << topic ); - if(enable_odom_prior) - { - std::string odom_prior_topic; - GetParamRequired( ph, "odom_prior_topic", odom_prior_topic ); - - _odomPriorSub = nh.subscribe( topic, - buffLen, - &TargetRegistration::OdometryPriorCallback, - this ); - - ROS_INFO_STREAM( "Target: " << _name << " subscribing to odometry prior on " - << odom_prior_topic ); - } _poses = std::make_shared( _graph, _optimizePose ); } @@ -225,21 +211,24 @@ bool TargetRegistration::IsPoseInitialized( const ros::Time& time ) const void TargetRegistration::OdometryCallback( const nav_msgs::Odometry::ConstPtr& msg ) { - PoseSE3::TangentVector vel = MsgToTangent( msg->twist.twist ); - PoseSE3::CovarianceMatrix cov; - ParseMatrix( msg->twist.covariance, cov ); // TODO Check return? - _velocityIntegrator.BufferInfo( msg->header.stamp.toSec(), vel, cov ); -} - -void TargetRegistration::OdometryPriorCallback( const nav_msgs::Odometry::ConstPtr& msg ) -{ - ros::Time time = msg->header.stamp; - PoseSE3 prior = MsgToPose( msg->pose.pose ); - PoseSE3::CovarianceMatrix cov; - ParseMatrix ( msg->pose.covariance, cov ); + if(_enableOdomPrior) + { + ros::Time time = msg->header.stamp; + PoseSE3 prior = MsgToPose( msg->pose.pose ); + PoseSE3::CovarianceMatrix cov; + ParseMatrix ( msg->pose.covariance, cov ); - isam::PoseSE3 p = PoseToIsam( prior ); - _poses->CreatePrior( time, p, isam::Covariance( cov ) ); + isam::PoseSE3 p = PoseToIsam( prior ); + _poses->CreatePrior( time, p, isam::Covariance( cov ) ); + + } + else + { + PoseSE3::TangentVector vel = MsgToTangent( msg->twist.twist ); + PoseSE3::CovarianceMatrix cov; + ParseMatrix( msg->twist.covariance, cov ); // TODO Check return? + _velocityIntegrator.BufferInfo( msg->header.stamp.toSec(), vel, cov ); + } } ExtrinsicsRegistration::ExtrinsicsRegistration( TargetRegistration& p,