Covariance evaluation

include/cfear_radarodometry/eval_trajectory.h

@@ -52,7 +52,15 @@ using namespace sensor_msgs;
 
 //EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::matrix<double,4,4>)
 
-typedef std::pair<Eigen::Affine3d, ros::Time> poseStamped;
+
+//typedef std::tuple<Eigen::Affine3d, Covariance, ros::Time> poseStamped;
+struct poseStamped {
+    Eigen::Affine3d pose;
+    Covariance cov;
+    ros::Time t;
+    poseStamped(){};
+    poseStamped(const Eigen::Affine3d & _pose, const Covariance & _cov, const ros::Time & _t):pose(_pose),cov(_cov),t(_t){};
+};
 typedef std::vector<poseStamped, Eigen::aligned_allocator<poseStamped>> poseStampedVector;
 typedef sync_policies::ApproximateTime<nav_msgs::Odometry, nav_msgs::Odometry> double_odom;
 
@@ -149,6 +157,10 @@ class EvalTrajectory
 
   void Write(const std::string& path, const poseStampedVector& v);
 
+  void WriteTUM(const std::string& path, const poseStampedVector& v);
+
+  void WriteCov(const std::string& path, const poseStampedVector& v);
+
   void PrintStatus();
 
   poseStampedVector::iterator FindElement(const ros::Time& t);

include/cfear_radarodometry/odometrykeyframefuser.h

@@ -181,6 +181,7 @@ class OdometryKeyframeFuser {
 protected:
   Eigen::Affine3d Tprev_fused, T_prev, Tmot;
   Eigen::Affine3d Tcurrent;
+  Covariance cov_current;
 
   // Components mat publishing
   boost::shared_ptr<n_scan_normal_reg> radar_reg = NULL;
@@ -208,6 +209,8 @@ class OdometryKeyframeFuser {
 
   void pointcloudCallback(pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_filtered,  pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_filtered_peaks,  Eigen::Affine3d &Tcurr, const ros::Time& t);
 
+  void pointcloudCallback(pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_filtered,  pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_filtered_peaks,  Eigen::Affine3d &Tcurr, const ros::Time& t, Covariance &cov_curr);
+
   std::string GetStatus(){return "Distance traveled: "+std::to_string(distance_traveled)+", nr sensor readings: "+std::to_string(frame_nr_);}
 
   void PrintSurface(const std::string& path, const Eigen::MatrixXd& surface);

launch/kvarntorp/run_sequence_kvarntorp

@@ -6,7 +6,7 @@ SEQUENCE="2020-09-18-14-38-03"
 current_date=`date '+%Y-%m-%d_%H:%M'`
 EVALUATION_description="cfear-x"
 
-BAG_BASE_PATH="/${BAG_LOCATION}/kvarntorp" #Change this to the directory where you have the bag files
+BAG_BASE_PATH="${BAG_LOCATION}/kvarntorp" #Change this to the directory where you have the bag files
 BAG_FILE_PATH="${BAG_BASE_PATH}/${SEQUENCE}.bag"
 echo "${BAG_FILE_PATH}"
 EVAL_BASE_DIR="${BAG_BASE_PATH}/eval/${current_date}"

src/cfear_radarodometry/eval_trajectory.cpp

@@ -1,4 +1,5 @@
 #include "cfear_radarodometry/eval_trajectory.h"
+#include <unsupported/Eigen/MatrixFunctions>
 
 namespace CFEAR_Radarodometry {
 
@@ -27,11 +28,15 @@ EvalTrajectory::EvalTrajectory(const EvalTrajectory::Parameters& pars, bool disa
 void EvalTrajectory::CallbackSynchronized(const nav_msgs::Odometry::ConstPtr& msg_est, const nav_msgs::Odometry::ConstPtr& msg_gt){
   cout<<"callback"<<endl;
   Eigen::Affine3d T;
+  Covariance cov;
+
+  cov = Eigen::Map<const Covariance>(msg_est->pose.covariance.data());       // the assignment makes a copy - safe
   tf::poseMsgToEigen(msg_est->pose.pose, T);
-  gt_vek.push_back(std::make_pair(T, msg_est->header.stamp));
+  gt_vek.push_back(poseStamped(T, cov, msg_est->header.stamp));
 
+  cov = Eigen::Map<const Covariance>(msg_gt->pose.covariance.data());
   tf::poseMsgToEigen(msg_gt->pose.pose, T);
-  est_vek.push_back(std::make_pair(T, msg_gt->header.stamp));
+  est_vek.push_back(poseStamped(T, cov, msg_gt->header.stamp));
 }
 void EvalTrajectory::CallbackEigen(const poseStamped& Test, const poseStamped& Tgt){
   gt_vek.push_back(Tgt);
@@ -48,17 +53,21 @@ void EvalTrajectory::CallbackESTEigen(const poseStamped& Test){
 
 void EvalTrajectory::CallbackGT(const nav_msgs::Odometry::ConstPtr &msg){
   Eigen::Affine3d T;
+  Covariance cov;
   ros::Time t = msg->header.stamp;
   tf::poseMsgToEigen(msg->pose.pose, T);
-  gt_vek.push_back(std::make_pair(T, t));
+  cov = Eigen::Map<const Covariance>(msg->pose.covariance.data());
+  gt_vek.push_back(poseStamped(T, cov, t));
 }
 
 
 void EvalTrajectory::CallbackEst(const nav_msgs::Odometry::ConstPtr &msg){
   Eigen::Affine3d T;
+  Covariance cov;
   tf::poseMsgToEigen(msg->pose.pose, T);
   ros::Time t = msg->header.stamp;
-  est_vek.push_back(std::make_pair(T, t));
+  cov = Eigen::Map<const Covariance>(msg->pose.covariance.data());
+  est_vek.push_back(poseStamped(T, cov, t));
 }
 
 std::string EvalTrajectory::DatasetToSequence(const std::string& dataset){
@@ -136,15 +145,15 @@ void EvalTrajectory::RemoveExtras(){
 
   while( !est_vek.empty() || !gt_vek.empty() ){
 
-    if( fabs( ros::Duration(est_vek.front().second - gt_vek.front().second).toSec() ) > 0.0001  ){
-      if(est_vek.front().second < gt_vek.front().second){
+    if( fabs( ros::Duration(est_vek.front().t - gt_vek.front().t).toSec() ) > 0.0001  ){
+      if(est_vek.front().t < gt_vek.front().t){
         est_vek.erase(est_vek.begin());
       }
       else
         gt_vek.erase(gt_vek.begin());
     }
-    else if( fabs( ros::Duration(est_vek.back().second - gt_vek.back().second).toSec() ) > 0.0001  ){
-      if(est_vek.back().second > gt_vek.back().second){
+    else if( fabs( ros::Duration(est_vek.back().t - gt_vek.back().t).toSec() ) > 0.0001  ){
+      if(est_vek.back().t > gt_vek.back().t){
         est_vek.pop_back();
       }
       else
@@ -161,15 +170,67 @@ void EvalTrajectory::Write(const std::string& path, const poseStampedVector& v){
   cout<<"Saving: "<<v.size()<<" poses to file: "<<path<<endl;
   evalfile.open(path);
   for(size_t i=0;i<v.size();i++){
-    Eigen::MatrixXd m(v[i].first.matrix());
-    evalfile<< std::fixed << std::showpoint;
+    // get the Affine3d matrix from the tuple
+    Eigen::MatrixXd m(v[i].pose.matrix());
+    // print to the file
+    evalfile << std::fixed << std::showpoint;
     assert(m.rows()== 4 && m.cols()==4);
-
-    evalfile<< MatToString(m) <<std::endl;
+    evalfile << MatToString(m) << endl;
   }
   evalfile.close();
   return;
 }
+
+void EvalTrajectory::WriteTUM(const std::string& path, const poseStampedVector& v){
+    std::ofstream evalfile;
+    cout<<"Saving: "<<v.size()<<" poses to file: "<<path<<endl;
+    evalfile.open(path);
+
+    for(size_t i=0;i<v.size();i++){
+        // print the timestamp
+        evalfile << v[i].t.sec << "." << std::setfill('0') << std::setw(9) << v[i].t.nsec << " " << std::setw(0);
+
+        // get the Affine3d pose and quaternion
+        // print the pose to the file
+        evalfile << std::fixed << std::setprecision(4);
+        evalfile << v[i].pose.translation().x() << " " << v[i].pose.translation().y() << " " << v[i].pose.translation().z() << " ";
+        Eigen::Quaterniond quat(v[i].pose.rotation());  // print as a quaternion
+        evalfile << std::defaultfloat;
+        evalfile << quat.x() << " " << quat.y() << " " << quat.z() << " " << quat.w(); // << " ";
+
+        // get the covariance and print it inline
+        //Eigen::IOFormat Inline_matrix_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ",
+        //                              " ", "", "", "", "");
+        //evalfile << v[i].cov.format(Inline_matrix_format) << " ";
+
+        evalfile << std::endl;
+    }
+    evalfile.close();
+    return;
+}
+
+
+void EvalTrajectory::WriteCov(const std::string& path, const poseStampedVector& v){
+    std::ofstream evalfile;
+    cout<<"Saving: "<<v.size()<<" covariances to file: "<<path<<endl;
+    evalfile.open(path);
+
+    for(size_t i=0;i<v.size();i++){
+        // print the timestamp
+        evalfile << v[i].t.sec << "." << std::setfill('0') << std::setw(9) << v[i].t.nsec << " " << std::setw(0);
+
+        // get the covariance and print it inline
+        Eigen::IOFormat Inline_matrix_format(Eigen::StreamPrecision, Eigen::DontAlignCols, " ",
+                                      " ", "", "", "", "");
+        evalfile << v[i].cov.format(Inline_matrix_format);
+
+        evalfile << std::endl;
+    }
+    evalfile.close();
+    return;
+}
+
+
 void Plot(){
 
 }
@@ -180,7 +241,7 @@ void EvalTrajectory::PublishTrajectory(poseStampedVector& vek, ros::Publisher& p
 
   std::vector<tf::StampedTransform> trans_vek;
   for (int i=0;i<vek.size();i++) {
-    Eigen::Affine3d T = vek[i].first;
+    Eigen::Affine3d T = vek[i].pose;
     geometry_msgs::PoseStamped Tstamped;
     tf::poseEigenToMsg(T,Tstamped.pose);
     path.poses.push_back(Tstamped);
@@ -191,11 +252,11 @@ void EvalTrajectory::PrintStatus(){
   if(gt_vek.size()!=est_vek.size()){
     //    std::cerr<<"SIZE ERROR. est_vek.size()="<<est_vek.size()<<", gt_vek.size()="<<gt_vek.size() <<std::endl;
   }
-  cout<<"Est first: "<<est_vek.front().first.translation().transpose()<<" - time: "<<est_vek.front().second<<endl;
-  cout<<"GT: first: "<<gt_vek.front().first.translation().transpose()<<" - time: "<<gt_vek.front().second<<endl;
+  cout<<"Est first: "<<est_vek.front().pose.translation().transpose()<<" - time: "<<est_vek.front().t<<endl;
+  cout<<"GT: first: "<<gt_vek.front().pose.translation().transpose()<<" - time: "<<gt_vek.front().t<<endl;
 
-  cout<<"Est Last: "<<est_vek.back().first.translation().transpose()<<" - time: "<<est_vek.back().second<<endl;
-  cout<<"GT: Last: "<<gt_vek.back().first.translation().transpose()<<" - time: "<<gt_vek.back().second<<endl;
+  cout<<"Est Last: "<<est_vek.back().pose.translation().transpose()<<" - time: "<<est_vek.back().t<<endl;
+  cout<<"GT: Last: "<<gt_vek.back().pose.translation().transpose()<<" - time: "<<gt_vek.back().t<<endl;
   return;
 }
 
@@ -211,10 +272,15 @@ void EvalTrajectory::Save(){
   else if(gt_vek.empty()){
     cout<<"No ground truth? No problem! without gps the need of radar based localization is even larger"<<endl;
     boost::filesystem::create_directories(par.est_output_dir);
-    std::string est_path = par.est_output_dir+DatasetToSequence(par.sequence);     cout<<"Saving estimate poses only, no ground truth, total: "<<est_vek.size()<<" poses"<<endl;
+    std::string est_path = par.est_output_dir+DatasetToSequence(par.sequence);
+    std::string est_path_tum = par.est_output_dir+ "tum_" +DatasetToSequence(par.sequence);
+    std::string est_path_cov = par.est_output_dir+ "cov_" +DatasetToSequence(par.sequence);
+    cout<<"Saving estimate poses only, no ground truth, total: "<<est_vek.size()<<" poses"<<endl;
     cout<<"To path: "<<est_path<<endl;
     Write(est_path,  est_vek);
-    cout<<"Trajectoy saved"<<endl;
+    WriteTUM(est_path_tum, est_vek);
+    WriteCov(est_path_cov, est_vek);
+    cout<<"Trajectory saved"<<endl;
     return;
 
   }else{
@@ -230,12 +296,18 @@ void EvalTrajectory::Save(){
   cout << "create: " << par.est_output_dir << std::endl;
   boost::filesystem::create_directories(par.est_output_dir);
   std::string gt_path  = par.gt_output_dir +DatasetToSequence(par.sequence);
+  std::string gt_path_tum  = par.gt_output_dir+ "tum_" +DatasetToSequence(par.sequence);
   std::string est_path = par.est_output_dir+DatasetToSequence(par.sequence);
+  std::string est_path_tum = par.est_output_dir + "tum_" + DatasetToSequence(par.sequence);
+  std::string est_path_cov = par.est_output_dir + "cov_" + DatasetToSequence(par.sequence);
   cout<<"Saving est_vek.size()="<<est_vek.size()<<", gt_vek.size()="<<gt_vek.size()<<endl;
   cout<<"To path: \n\" "<<gt_path<<"\""<<"\n\""<<est_path<<"\""<<endl;
 
   Write(gt_path, gt_vek);
+  WriteTUM(gt_path_tum, gt_vek);
   Write(est_path,  est_vek);
+  WriteTUM(est_path_tum,  est_vek);
+  WriteCov(est_path_cov,  est_vek);
   cout<<"Trajectories saved"<<endl;
 
   return;
@@ -331,7 +403,7 @@ void EvalTrajectory::One2OneCorrespondance(){
   poseStampedVector::iterator init_guess = gt_vek.begin();
   for(size_t i=0 ; i<est_vek.size() ; i++){
     poseStamped interp_corr;
-    if(Interpolate(est_vek[i].second, interp_corr, init_guess)){
+    if(Interpolate(est_vek[i].t, interp_corr, init_guess)){
       revised_est.push_back(est_vek[i]);
       revised_gt.push_back(interp_corr);
     }
@@ -357,8 +429,8 @@ bool EvalTrajectory::SearchByTime(const ros::Time& t, poseStampedVector::iterato
   poseStampedVector::iterator last = std::prev(gt_vek.end());
 
   for(auto &&it = itr ; itr != last; it++){
-    double tprev = it->second.toSec();
-    double tnext = std::next(it)->second.toSec();
+    double tprev = it->t.toSec();
+    double tnext = std::next(it)->t.toSec();
     if(tprev <= tsearch && tsearch <= tnext ) {
       itr = it;
       //cout<<"min:"<<it->second<<" search: "<<t<<"max: "<<std::next(it)->second<<endl;
@@ -374,9 +446,12 @@ bool EvalTrajectory::Interpolate(const ros::Time& t, poseStamped& Tinterpolated,
     poseStamped Tbefore = *itr;
     poseStamped Tafter = *std::next(itr);
 
-    Tinterpolated.first = pose_interp(t.toSec(), Tbefore.second.toSec(), Tafter.second.toSec(),
-                                      Tbefore.first, Tafter.first);
-    Tinterpolated.second = t;
+      Tinterpolated.pose = pose_interp(t.toSec(), Tbefore.t.toSec(), Tafter.t.toSec(),
+                                               Tbefore.pose, Tafter.pose);
+      Tinterpolated.t = t;
+      //TODO: This should be a geometric mean instead to do the interpolation correctly: https://www.lyndonduong.com/psd-cone/
+      //However, we don't have GT's covariance anyway, so we would be computing all the stuff (3x matrix sqrt) for nothing
+      Tinterpolated.cov = Tbefore.cov;
     return true;
   }
   else return false;

src/cfear_radarodometry/odometrykeyframefuser.cpp

@@ -30,6 +30,7 @@ OdometryKeyframeFuser::OdometryKeyframeFuser(const Parameters& pars, bool disabl
 
   Tprev_fused = Eigen::Affine3d::Identity();
   Tcurrent = Eigen::Affine3d::Identity();
+  cov_current = Covariance::Identity();
   T_prev = Eigen::Affine3d::Identity();
   Tmot = Eigen::Affine3d::Identity();
 
@@ -186,6 +187,7 @@ void OdometryKeyframeFuser::processFrame(pcl::PointCloud<pcl::PointXYZI>::Ptr& c
   }
 
   Tcurrent = T_vek.back();
+  cov_current = cov_vek.back();
   Eigen::Affine3d Tmot_current = T_prev.inverse()*Tcurrent;
   if(!AccelerationVelocitySanityCheck(Tmot, Tmot_current))
     Tcurrent = Tguess; //Insane acceleration and speed, lets use the guess.
@@ -194,7 +196,7 @@ void OdometryKeyframeFuser::processFrame(pcl::PointCloud<pcl::PointXYZI>::Ptr& c
 
   MapPointNormal::PublishMap("/current_normals", Pcurrent, Tcurrent, par.odometry_link_id,-1,0.5);
   pcl::PointCloud<pcl::PointXYZI> cld_latest = FormatScanMsg(*cloud, Tcurrent);
-  nav_msgs::Odometry msg_current = FormatOdomMsg(Tcurrent, Tmot, t, cov_vek.back());
+  nav_msgs::Odometry msg_current = FormatOdomMsg(Tcurrent, Tmot, t, cov_current);
   pubsrc_cloud_latest.publish(cld_latest);
   pose_current_publisher.publish(msg_current);
   if(par.publish_tf_){
@@ -269,6 +271,12 @@ void OdometryKeyframeFuser::pointcloudCallback(pcl::PointCloud<pcl::PointXYZI>::
 
 }
 
+void OdometryKeyframeFuser::pointcloudCallback(pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_filtered,  pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_filtered_peaks,  Eigen::Affine3d &Tcurr, const ros::Time& t, Covariance &cov_curr){
+    pointcloudCallback(cloud_filtered, cloud_filtered_peaks, Tcurr, t);
+    cov_curr = cov_current;
+}
+
+
 void OdometryKeyframeFuser::PrintSurface(const std::string& path, const Eigen::MatrixXd& surface){
   std::ofstream myfile;
   myfile.open (path);
@@ -304,7 +312,7 @@ void OdometryKeyframeFuser::AddToGraph(PoseScanVector& reference, RadarScan& sca
 void OdometryKeyframeFuser::AddGroundTruth(poseStampedVector& gt_vek){
   std::map<unsigned long,Pose3d> stamp_map;
   for (auto && gt : gt_vek ){ // construct map of ground truth poses
-    stamp_map[gt.second.toNSec()] = PoseEigToCeres(gt.first);
+    stamp_map[gt.t.toNSec()] = PoseEigToCeres(gt.pose);
     //cout << "gt : " << gt.second.toNSec() << endl;
   }
 

src/offline_odometry.cpp

@@ -79,14 +79,14 @@ class radarReader
       nav_msgs::Odometry::ConstPtr odom_msg = m.instantiate<nav_msgs::Odometry>();
       if (odom_msg != NULL){
         nav_msgs::Odometry msg_odom = *odom_msg;
-        poseStamped stamped_gt_pose = std::make_pair(Eigen::Affine3d::Identity(), odom_msg->header.stamp);
-        tf::poseMsgToEigen(msg_odom.pose.pose, stamped_gt_pose.first);
-        stamped_gt_pose.first = stamped_gt_pose.first;//transform into sensor frame
-        Eigen::Matrix4d m = stamped_gt_pose.first.matrix();
+        poseStamped stamped_gt_pose(Eigen::Affine3d::Identity(), Covariance::Identity(), odom_msg->header.stamp);
+        tf::poseMsgToEigen(msg_odom.pose.pose, stamped_gt_pose.pose);
+        //stamped_gt_pose.pose = stamped_gt_pose.pose;//transform into sensor frame
+        Eigen::Matrix4d m = stamped_gt_pose.pose.matrix();
         m(0,2) = 0; m(2,0) = 0; m(2,1) = 0; m(1,2) = 0; m(2,2) = 1; // 3d -> 2d
-        stamped_gt_pose.first = Eigen::Affine3d(m);
-        static Eigen::Affine3d Tfirst_i = stamped_gt_pose.first.inverse();
-        stamped_gt_pose.first = Tfirst_i*stamped_gt_pose.first;
+        stamped_gt_pose.pose = Eigen::Affine3d(m);
+        static Eigen::Affine3d Tfirst_i = stamped_gt_pose.pose.inverse();
+        stamped_gt_pose.pose = Tfirst_i*stamped_gt_pose.pose;
         eval.CallbackGTEigen(stamped_gt_pose);
 
         msg_odom.header.stamp = ros::Time::now();
@@ -103,8 +103,9 @@ class radarReader
         driver.CallbackOffline(image_msg, cloud_filtered, cloud_filtered_peaks);
         CFEAR_Radarodometry::timing.Document("Filtered points",cloud_filtered->size());
         Eigen::Affine3d Tcurrent;
+        Covariance cov_current;
 
-        fuser.pointcloudCallback(cloud_filtered, cloud_filtered_peaks, Tcurrent, image_msg->header.stamp);
+        fuser.pointcloudCallback(cloud_filtered, cloud_filtered_peaks, Tcurrent, image_msg->header.stamp, cov_current);
         if(fuser.updated && p.save_radar_img){
           std::string path = eval_par.est_output_dir + "/" + std::to_string(image_msg->header.stamp.toNSec())+".png";
           cv::imwrite(path, driver.cv_polar_image->image);
@@ -113,7 +114,7 @@ class radarReader
 
 
 
-        eval.CallbackESTEigen(std::make_pair(Tcurrent, image_msg->header.stamp));
+        eval.CallbackESTEigen(poseStamped(Tcurrent, cov_current, image_msg->header.stamp));
         ros::Time tnow = ros::Time::now();
         ros::Duration d = ros::Duration(tnow-tinit);
         static ros::Duration tot(0);