Perturbations and covariance sampling

README.md

@@ -27,7 +27,7 @@ cd /home/${USER}/Documents/oxford-eval-sequences/2019-01-10-12-32-52-radar-oxfor
 ## Running
 The odometry can be launched in two modes.
 * Offline: (used in this guide) runs at the maximum possible rate.
-* Online: Standard rostopic interface. radar_driver_node and odometry_keyframes. The radar_driver_node subscribes to "/Navtech/Polar" 
+* Online (not tested): Standard rostopic interface. radar_driver_node and odometry_keyframes. The radar_driver_node subscribes to "/Navtech/Polar" 
 
 ```
 roscd cfear_radarodometry/launch

include/cfear_radarodometry/n_scan_normal.h

@@ -46,6 +46,8 @@ class n_scan_normal_reg : public Registration{
 
   double getScore(){return score_;}
 
+  bool GetCovarianceScaler(double &cov_scale);
+
   void getScore(double& score, int& num_residuals){score = score_; num_residuals = problem_->NumResiduals();}
 
   void SetD2dPar(const double cov_scale,const double regularization){cov_scale_ = cov_scale; regularization_ = regularization;}

include/cfear_radarodometry/odometrykeyframefuser.h

@@ -27,6 +27,7 @@
 #include <time.h>
 #include <cstdio>
 
+
 #include <pcl_ros/transforms.h>
 #include "pcl_ros/publisher.h"
 
@@ -55,6 +56,7 @@ using std::endl;
 
 
 
+
 namespace CFEAR_Radarodometry {
 
 
@@ -99,9 +101,20 @@ class OdometryKeyframeFuser {
     double covar_scale_ = 1.0;
     double regularization_ = 0.0;
 
+    bool estimate_cov_by_sampling = false;
+    bool cov_samples_to_file_as_well = false; // Will save in the desired folder
+    std::string cov_sampling_file_directory = "/tmp/cfear_out";
+    double cov_sampling_xy_range = 0.4;  // Will sample from -0.2 to +0.2
+    double cov_sampling_yaw_range = 0.0043625; //Will sample from -half to +half of this range as well
+    unsigned int cov_sampling_samples_per_axis = 3; //Will do 5^3 in the end
+    double cov_sampling_covariance_scaler = 4.0;
+
+
     bool publish_tf_ = true;
     bool store_graph = false;
 
+
+
     void GetParametersFromRos( ros::NodeHandle& param_nh){
       param_nh.param<std::string>("input_points_topic", input_points_topic, "/Navtech/Filtered");
       param_nh.param<std::string>("scan_registered_latest_topic", scan_registered_latest_topic, "/radar_registered");
@@ -170,6 +183,13 @@ class OdometryKeyframeFuser {
       stringStream << "publish_tf, "<<std::boolalpha<<publish_tf_<<endl;
       stringStream << "store graph, "<<store_graph<<endl;
       stringStream << "Weight, "<<weight_opt<<endl;
+      stringStream << "Use cost sampling for covariance, "<<std::boolalpha<<estimate_cov_by_sampling<<endl;
+      stringStream << "Save cost samples to a file, "<<std::boolalpha<<cov_samples_to_file_as_well<<endl;
+      stringStream << "Cost-samples-file folder, "<<cov_sampling_file_directory<<endl;
+      stringStream << "XY sampling range, "<<cov_sampling_xy_range<<endl;
+      stringStream << "Yaw sampling range, "<<cov_sampling_yaw_range<<endl;
+      stringStream << "Cost samples per axis, "<<cov_sampling_samples_per_axis<<endl;
+      stringStream << "Sampled covariance scale, "<<cov_sampling_covariance_scaler<<endl;
       return stringStream.str();
     }
   };
@@ -239,6 +259,8 @@ class OdometryKeyframeFuser {
 
   void pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& cloud_filtered);
 
+  bool approximateCovarianceBySampling(std::vector<CFEAR_Radarodometry::MapNormalPtr> &scans_vek, const std::vector<Eigen::Affine3d> &T_vek, Covariance &cov_sampled);
+
 
 
 
@@ -255,4 +277,8 @@ void FormatScans(const PoseScanVector& reference,
                  std::vector<Eigen::Affine3d>& T_vek
                  );
 
+template<typename T> std::vector<double> linspace(T start_in, T end_in, int num_in);
+
 }
+
+

launch/kvarntorp/run_sequence_kvarntorp

@@ -69,8 +69,10 @@ export dataset="kvarntorp"
 export LOSS_TYPE="Huber"
 export LOSS_LIMIT="0.1"
 
+# COV SAMPLING PARS
+export covar_sampling="False"
 
-pars="--range-res ${range_res} --sequence ${SEQUENCE} --radar_ccw ${radar_ccw} --soft_constraint ${soft_constraint} --disable_compensate ${disable_compensate} --cost_type ${cost_type} --submap_scan_size ${submap_scan_size} --registered_min_keyframe_dist ${registered_min_keyframe_dist} --res ${res} --k_strongest ${kstrong} --bag_path ${BAG_FILE_PATH} --est_directory ${est_dir} --gt_directory ${gt_dir} --job_nr 1 --z-min ${zmin} --loss_type ${LOSS_TYPE} --loss_limit ${LOSS_LIMIT}  --weight_intensity ${weight_intensity} --method ${EVALUATION_description} --weight_option ${weight_option} --dataset ${dataset}"
+pars="--range-res ${range_res} --sequence ${SEQUENCE} --radar_ccw ${radar_ccw} --soft_constraint ${soft_constraint} --disable_compensate ${disable_compensate} --cost_type ${cost_type} --submap_scan_size ${submap_scan_size} --registered_min_keyframe_dist ${registered_min_keyframe_dist} --res ${res} --k_strongest ${kstrong} --bag_path ${BAG_FILE_PATH} --est_directory ${est_dir} --gt_directory ${gt_dir} --job_nr 1 --z-min ${zmin} --loss_type ${LOSS_TYPE} --loss_limit ${LOSS_LIMIT}  --weight_intensity ${weight_intensity} --method ${EVALUATION_description} --weight_option ${weight_option} --dataset ${dataset} --store_graph true --covar_sampling ${covar_sampling}"
 echo "|||||Estimating odometry with parameters: ${pars}||||"
 roslaunch cfear_radarodometry vis.launch&
 rosrun cfear_radarodometry offline_odometry ${pars} #>/dev/null

launch/oxford/run_sequence_save_odom

@@ -55,11 +55,13 @@ export radar_ccw="false" #False for oxford, otherwise true
 export soft_constraint="false"
 export disable_compensate="false"
 export dataset="oxford"
-export LOSS_TYPE="Huber"
+export LOSS_TYPE="Huber" #"Huber" "Cauchy" 
 export LOSS_LIMIT="0.1"
 
+# COV SAMPLING PARS
+export covar_sampling="False"
 
-pars="--range-res ${range_res} --sequence ${SEQUENCE} --radar_ccw ${radar_ccw} --soft_constraint ${soft_constraint} --disable_compensate ${disable_compensate} --cost_type ${cost_type} --submap_scan_size ${submap_scan_size} --registered_min_keyframe_dist ${registered_min_keyframe_dist} --res ${res} --k_strongest ${kstrong} --bag_path ${BAG_FILE_PATH} --est_directory ${est_dir} --gt_directory ${gt_dir} --job_nr 1 --z-min ${zmin} --loss_type ${LOSS_TYPE} --loss_limit ${LOSS_LIMIT}  --weight_intensity ${weight_intensity} --method ${EVALUATION_description} --weight_option ${weight_option} --dataset ${dataset} --store_graph true --save_radar_img true"
+pars="--range-res ${range_res} --sequence ${SEQUENCE} --radar_ccw ${radar_ccw} --soft_constraint ${soft_constraint} --disable_compensate ${disable_compensate} --cost_type ${cost_type} --submap_scan_size ${submap_scan_size} --registered_min_keyframe_dist ${registered_min_keyframe_dist} --res ${res} --k_strongest ${kstrong} --bag_path ${BAG_FILE_PATH} --est_directory ${est_dir} --gt_directory ${gt_dir} --job_nr 1 --z-min ${zmin} --loss_type ${LOSS_TYPE} --loss_limit ${LOSS_LIMIT}  --weight_intensity ${weight_intensity} --method ${EVALUATION_description} --weight_option ${weight_option} --dataset ${dataset} --store_graph true --save_radar_img true --covar_sampling ${covar_sampling}"
 echo "|||||Estimating odometry with parameters: ${pars}||||"
 roslaunch cfear_radarodometry vis.launch&
 rosrun cfear_radarodometry offline_odometry ${pars} #>/dev/null

src/cfear_radarodometry/n_scan_normal.cpp

@@ -118,7 +118,8 @@ bool n_scan_normal_reg::Register(std::vector<MapNormalPtr>& scans, std::vector<E
     //  FOr visualization only
     if(success)
       for(size_t i = 0 ; i<n_scans ; i++)
-        Tsrc[i] = vectorToAffine3d(parameters[i]);
+          Tsrc[i] = vectorToAffine3d(parameters[i]);
+
     double current_score = summary_.final_cost;
     const double rel_improvement = (prev_score-current_score)/prev_score;
     const Eigen::Vector2d trans_diff(parameters.back()[0] - prev_par[0],parameters.back()[1] - prev_par[1]);
@@ -412,7 +413,16 @@ bool n_scan_normal_reg::GetCovariance(Matrix6d &Cov){
   else{
     double covariance_xx[3 * 3];
     covariance.GetCovarianceBlock(v, v, covariance_xx);
-    Eigen::MatrixXd cmat = 10*Eigen::Map<Eigen::Matrix<double,3,3> >(covariance_xx);
+    //DONE BUT IT IS BAD: Should be scaled by num. of residuals and score (Andrea Censi 2007, (3))
+    if(summary_.num_residuals_reduced-summary_.num_parameters_reduced==0) return false;
+    Eigen::MatrixXd cmat = 30*(summary_.final_cost/(summary_.num_residuals_reduced-summary_.num_parameters_reduced))
+            * Eigen::Map<Eigen::Matrix<double,3,3>>(covariance_xx);
+    //  cout << "Covariance scaling: " << 30.0*(summary_.final_cost/(summary_.num_residuals_reduced-summary_.num_parameters_reduced))
+    //       << endl;
+
+    // The original way by Daniel:
+    // Eigen::MatrixXd cmat =  2.0 * Eigen::Map<Eigen::Matrix<double,3,3> >(covariance_xx); // Magic constat based on data
+
     Cov = Eigen::Matrix<double,6,6>::Identity();
     Cov.block<2,2>(0,0) = cmat.block<2,2>(0,0);
     Cov(5,5) = cmat(2,2);
@@ -421,6 +431,15 @@ bool n_scan_normal_reg::GetCovariance(Matrix6d &Cov){
     return true;
   }
 }
+
+bool n_scan_normal_reg::GetCovarianceScaler(double &cov_scale){
+    //Cov should be scaled by num. of residuals and score (Andrea Censi 2007, (3))
+    if(summary_.num_residuals_reduced-summary_.num_parameters_reduced==0) return false;
+
+    cov_scale = summary_.final_cost/(summary_.num_residuals_reduced-summary_.num_parameters_reduced);
+    return true;
+}
+
 bool n_scan_normal_reg::SolveOptimizationProblem(){
 
   CHECK(problem_ != nullptr);

src/cfear_radarodometry/odometrykeyframefuser.cpp

@@ -1,4 +1,7 @@
 #include "cfear_radarodometry/odometrykeyframefuser.h"
+#include <Eigen/Dense>
+#include <Eigen/Eigenvalues>
+
 namespace CFEAR_Radarodometry {
 
 visualization_msgs::Marker GetDefault(){
@@ -17,7 +20,7 @@ visualization_msgs::Marker GetDefault(){
   return m;
 }
 
-OdometryKeyframeFuser::OdometryKeyframeFuser(const Parameters& pars, bool disable_callback) : par(pars), nh_("~"){
+OdometryKeyframeFuser::OdometryKeyframeFuser(const Parameters& pars, bool disable_callback) : par(pars), nh_("~") {
   assert (!par.input_points_topic.empty() && !par.scan_registered_latest_topic.empty() && !par.scan_registered_keyframe_topic.empty() && !par.odom_latest_topic.empty() && !par.odom_keyframe_topic.empty() );
   assert(par.res>0.05 && par.submap_scan_size>=1 );
   //radar_reg =
@@ -139,7 +142,6 @@ pcl::PointCloud<pcl::PointXYZI> OdometryKeyframeFuser::FormatScanMsg(pcl::PointC
 
 void OdometryKeyframeFuser::processFrame(pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud, pcl::PointCloud<pcl::PointXYZI>::Ptr& cloud_peaks, const ros::Time& t) {
 
-
   ros::Time t0 = ros::Time::now();
   Eigen::Affine3d TprevMot(Tmot);
   if(par.compensate){
@@ -149,11 +151,12 @@ void OdometryKeyframeFuser::processFrame(pcl::PointCloud<pcl::PointXYZI>::Ptr& c
 
 
   std::vector<Matrix6d> cov_vek;
+  Covariance cov_sampled;
   std::vector<CFEAR_Radarodometry::MapNormalPtr> scans_vek;
   std::vector<Eigen::Affine3d> T_vek;
   ros::Time t1 = ros::Time::now();
-  CFEAR_Radarodometry::MapNormalPtr Pcurrent = CFEAR_Radarodometry::MapNormalPtr(new MapPointNormal(cloud, par.res, Eigen::Vector2d(0,0), par.weight_intensity_, par.use_raw_pointcloud));
 
+  CFEAR_Radarodometry::MapNormalPtr Pcurrent = CFEAR_Radarodometry::MapNormalPtr(new MapPointNormal(cloud, par.res, Eigen::Vector2d(0,0), par.weight_intensity_, par.use_raw_pointcloud));
 
   ros::Time t2 = ros::Time::now();
   Eigen::Affine3d Tguess;
@@ -162,6 +165,7 @@ void OdometryKeyframeFuser::processFrame(pcl::PointCloud<pcl::PointXYZI>::Ptr& c
   else
     Tguess = T_prev;
 
+
   if(keyframes_.empty()){
     scan_ = RadarScan(Eigen::Affine3d::Identity(), Eigen::Affine3d::Identity(), cloud_peaks, cloud, Pcurrent, t);
     AddToGraph(keyframes_, scan_, Eigen::Matrix<double,6,6>::Identity());
@@ -193,6 +197,13 @@ void OdometryKeyframeFuser::processFrame(pcl::PointCloud<pcl::PointXYZI>::Ptr& c
     Tcurrent = Tguess; //Insane acceleration and speed, lets use the guess.
   Tmot = T_prev.inverse()*Tcurrent;
 
+  // Try to approximate the covariance by the cost-sampling approach
+  if(par.estimate_cov_by_sampling){
+      if(approximateCovarianceBySampling(scans_vek, T_vek, cov_sampled)){ // if success, cov_sampled contains valid data
+          cov_current = cov_sampled;
+          cov_vek.back() = cov_sampled;
+      }
+  }
 
   MapPointNormal::PublishMap("/current_normals", Pcurrent, Tcurrent, par.odometry_link_id,-1,0.5);
   pcl::PointCloud<pcl::PointXYZI> cld_latest = FormatScanMsg(*cloud, Tcurrent);
@@ -229,7 +240,7 @@ void OdometryKeyframeFuser::processFrame(pcl::PointCloud<pcl::PointXYZI>::Ptr& c
 
     frame_nr_++;
     scan_ = RadarScan(Tcurrent, TprevMot, cloud_peaks, cloud, Pcurrent, t);
-    AddToGraph(keyframes_, scan_, Eigen::Matrix<double,6,6>::Identity());
+    AddToGraph(keyframes_, scan_, cov_current);
     AddToReference(keyframes_,scan_, par.submap_scan_size);
     updated = true;
 
@@ -245,6 +256,127 @@ void OdometryKeyframeFuser::processFrame(pcl::PointCloud<pcl::PointXYZI>::Ptr& c
 
 }
 
+bool OdometryKeyframeFuser::approximateCovarianceBySampling(std::vector<CFEAR_Radarodometry::MapNormalPtr> &scans_vek,
+                                                            const std::vector<Eigen::Affine3d> &T_vek,
+                                                            Covariance &cov_sampled){
+  bool cov_sampled_success = true;
+  string path;   // for dumping samples to a file
+  std::ofstream cov_samples_file;
+
+  std::vector<Eigen::Affine3d> T_vek_copy(T_vek);
+  Eigen::Affine3d T_best_guess = T_vek_copy.back();
+
+  if(par.cov_samples_to_file_as_well){
+    path = par.cov_sampling_file_directory + string("/cov_samples_") + std::to_string(nr_callbacks_) + string(".csv");
+    cov_samples_file.open(path);
+  }
+
+  double xy_sample_range = par.cov_sampling_xy_range*0.5; // sampling in x and y axis around the estimated pose
+  double theta_range = par.cov_sampling_yaw_range*0.5;  // sampling on the yaw axis plus minus
+  unsigned int number_of_steps_per_axis = par.cov_sampling_samples_per_axis;
+  unsigned int number_of_samples = number_of_steps_per_axis*number_of_steps_per_axis*number_of_steps_per_axis;
+
+  Eigen::Affine3d sample_T = Eigen::Affine3d::Identity();
+  double sample_cost = 0;  // return of the getCost function
+  std::vector<double> residuals; // return of the getCost function
+  Eigen::VectorXd samples_x_values(number_of_samples);
+  Eigen::VectorXd samples_y_values(number_of_samples);
+  Eigen::VectorXd samples_yaw_values(number_of_samples);
+  Eigen::VectorXd samples_cost_values(number_of_samples);
+
+  std::vector<double> xy_samples = linspace<double>(-xy_sample_range, xy_sample_range, number_of_steps_per_axis);
+  std::vector<double> theta_samples = linspace<double>(-theta_range, theta_range, number_of_steps_per_axis);
+
+  // Sample the cost function according to the settings, optionally dump the samples into a file
+  int vector_pointer = 0;
+  for (int theta_sample_id = 0; theta_sample_id < number_of_steps_per_axis; theta_sample_id++) {
+    for (int x_sample_id = 0; x_sample_id < number_of_steps_per_axis; x_sample_id++) {
+      for (int y_sample_id = 0; y_sample_id < number_of_steps_per_axis; y_sample_id++) {
+
+        sample_T.translation() = Eigen::Vector3d(xy_samples[x_sample_id],
+                                                 xy_samples[y_sample_id],
+                                                 0.0) + T_best_guess.translation();
+        sample_T.linear() = Eigen::AngleAxisd(theta_samples[theta_sample_id],
+                                              Eigen::Vector3d(0.0, 0.0, 1.0)) * T_best_guess.linear();
+
+        T_vek_copy.back() = sample_T;
+        radar_reg->GetCost(scans_vek, T_vek_copy, sample_cost, residuals);
+
+        samples_x_values[vector_pointer] = xy_samples[x_sample_id];
+        samples_y_values[vector_pointer] = xy_samples[y_sample_id];
+        samples_yaw_values[vector_pointer] = theta_samples[theta_sample_id];
+        samples_cost_values[vector_pointer] = sample_cost;
+        vector_pointer ++;
+
+        if(par.cov_samples_to_file_as_well) {
+            cov_samples_file << xy_samples[x_sample_id] << " " << xy_samples[y_sample_id] <<
+                             " " << theta_samples[theta_sample_id] << " " << sample_cost << std::endl;
+        }
+      }
+    }
+  }
+  if(cov_samples_file.is_open()) cov_samples_file.close();
+
+  //Find the approximating quadratic function by linear least squares
+  // f(x,y,z) = ax^2 + by^2 + cz^2 + dxy + eyz + fzx + gx+ hy + iz + j
+  // A = [x^2, y^2, z^2, xy, yz, zx, x, y, z, 1]
+  Eigen::MatrixXd A(number_of_samples, 10);
+  A << samples_x_values.array().square().matrix(),
+          samples_y_values.array().square().matrix(),
+          samples_yaw_values.array().square().matrix(),
+          (samples_x_values.array()*samples_y_values.array()).matrix(),
+          (samples_y_values.array()*samples_yaw_values.array()).matrix(),
+          (samples_yaw_values.array()*samples_x_values.array()).matrix(),
+          samples_x_values,
+          samples_y_values,
+          samples_yaw_values,
+          Eigen::MatrixXd::Ones(number_of_samples,1);
+
+  Eigen::VectorXd quad_func_coefs = A.bdcSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(samples_cost_values);
+
+  // With the coefficients, we can contruct the Hessian matrix (constant for a given function)
+  Eigen::Matrix3d hessian_matrix;
+  hessian_matrix << 2*quad_func_coefs[0],   quad_func_coefs[3],   quad_func_coefs[5],
+          quad_func_coefs[3], 2*quad_func_coefs[1],   quad_func_coefs[4],
+          quad_func_coefs[5],   quad_func_coefs[4], 2*quad_func_coefs[2];
+
+  // We need to check if the approximating function is convex (all eigs positive, we don't went the zero eigs either)
+  Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> eigensolver(hessian_matrix);
+  Eigen::Vector3d eigValues;
+  Eigen::Matrix3d covariance_3x3;
+
+  if (eigensolver.info() != Eigen::Success) {
+    cov_sampled_success = false;
+    std::cout << "Covariance sampling warning: Eigenvalues search failed." << std::endl;
+  }
+  else{
+    eigValues = eigensolver.eigenvalues();
+    if(eigValues[0] <= 0.0 || eigValues[1] <= 0.0 || eigValues[2] <= 0.0){
+        cov_sampled_success = false;
+        std::cout << "Covariance sampling warning: Quadratic approximation not convex. Sampling will not be used for this scan." << std::endl;
+    }
+    else{
+      // Compute the covariance from the hessian and scale by the score
+      double score_scale = 1.0;
+      if(radar_reg->GetCovarianceScaler(score_scale)) {
+
+        covariance_3x3 = 2.0 * hessian_matrix.inverse() * score_scale * par.cov_sampling_covariance_scaler;
+
+        //We need to construct the full 6DOF cov matrix
+        cov_sampled = Eigen::Matrix<double, 6, 6>::Identity();
+        cov_sampled.block<2, 2>(0, 0) = covariance_3x3.block<2, 2>(0, 0);
+        cov_sampled(5, 5) = covariance_3x3(2, 2);
+        cov_sampled(0, 5) = covariance_3x3(0, 2);
+        cov_sampled(1, 5) = covariance_3x3(1, 2);
+        cov_sampled(5, 0) = covariance_3x3(2, 0);
+        cov_sampled(5, 1) = covariance_3x3(2, 1);
+      }
+      else cov_sampled_success = false;
+    }
+  }
+  return cov_sampled_success;
+}
+
 void OdometryKeyframeFuser::pointcloudCallback(const sensor_msgs::PointCloud2::ConstPtr& msg_in){
   updated = false;
   //  cout<<"callback"<<endl;
@@ -360,4 +492,33 @@ void FormatScans(const PoseScanVector& reference,
 }
 
 
+
+template<typename T>
+std::vector<double> linspace(T start_in, T end_in, int num_in)
+{
+
+    std::vector<double> linspaced;
+
+    double start = static_cast<double>(start_in);
+    double end = static_cast<double>(end_in);
+    double num = static_cast<double>(num_in);
+
+    if (num == 0) { return linspaced; }
+    if (num == 1)
+    {
+        linspaced.push_back(start);
+        return linspaced;
+    }
+
+    double delta = (end - start) / (num - 1);
+
+    for(int i=0; i < num-1; ++i)
+    {
+        linspaced.push_back(start + delta * i);
+    }
+    linspaced.push_back(end); // I want to ensure that start and end
+    // are exactly the same as the input
+    return linspaced;
+}
+
 }