remove duplicate median function

tmcg0 · Dec 6, 2023 · 80c3b29 · 80c3b29
1 parent a18fcdd
commit 80c3b29
Show file tree

Hide file tree

Showing 3 changed files with 3 additions and 24 deletions.
diff --git a/include/gtsamutils.h b/include/gtsamutils.h
@@ -30,7 +30,6 @@ namespace gtsamutils{
  gtsam::Vector3 gyros_Vector3(const imu& myImu, const int& idx);
  Eigen::MatrixXd gyroMatrix(const imu& myImu);
  Eigen::MatrixXd accelMatrix(const imu& myImu);
- double median(std::vector<double> v);
  uint nearestIdxToVal(std::vector<double> v, double val);
  void saveMatrixToFile(const gtsam::Matrix& A, const std::string &s, const std::string& filename);
  void writeEigenMatrixToCsvFile(const std::string& name, const Eigen::MatrixXd& matrix, const Eigen::IOFormat& CSVFormat=Eigen::IOFormat(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n"));

diff --git a/src/gtsamutils.cpp b/src/gtsamutils.cpp
@@ -154,27 +154,7 @@ namespace gtsamutils{
  }
  return accels;
  }
-
- double median(std::vector<double> len){
- assert(!len.empty());
- if (len.size() % 2 == 0) {
- const auto median_it1 = len.begin() + len.size() / 2 - 1;
- const auto median_it2 = len.begin() + len.size() / 2;
-
- std::nth_element(len.begin(), median_it1 , len.end());
- const auto e1 = *median_it1;
-
- std::nth_element(len.begin(), median_it2 , len.end());
- const auto e2 = *median_it2;
-
- return (e1 + e2) / 2;
-
- } else {
- const auto median_it = len.begin() + len.size() / 2;
- std::nth_element(len.begin(), median_it , len.end());
- return *median_it;
- }
- }
+
 
  uint nearestIdxToVal(std::vector<double> v, double val){
  // this is gonna be ugly. copies entire vector and brute force searches for index nearest to value val.

diff --git a/src/lowerBodyPoseEstimator.cpp b/src/lowerBodyPoseEstimator.cpp
@@ -1897,7 +1897,7 @@ void lowerBodyPoseEstimator::adjustDistalImuTrajectoryForInboundJointAngles(cons
  if(zeroMedianCenterIntExtRot){
  Eigen::VectorXd intExtRotAngs=origJointAngles.col(2);
  std::vector<double> vec(intExtRotAngs.data(), intExtRotAngs.data() + intExtRotAngs.rows() * intExtRotAngs.cols()); // cast as std::vector
- double myMedian=gtsamutils::median(vec);
+ double myMedian=mathutils::median(vec);
  uint medIdx=gtsamutils::nearestIdxToVal(vec, myMedian);
  //std::cout<<std::endl<<"int/ext rot median is "<<myMedian<<" and occurs at index "<<medIdx<<" (true val in array = "<<vec[medIdx]<<")"<<std::endl;
  R_B_to_B2 = lowerBodyPoseEstimator::getDistalImuOrientationAdjustmentGivenDesiredIntExtRotAngle(R_ProxSeg_to_N[medIdx],distalImuPose[medIdx].rotation(),0.0,rightAxisDist,distalImuToProxJointCtr,distalImuToDistalJointCtr); // R[B->B2]
@@ -1911,7 +1911,7 @@ void lowerBodyPoseEstimator::adjustDistalImuTrajectoryForInboundJointAngles(cons
  if(zeroMedianCenterAbAd){
  Eigen::VectorXd abAdAngs=origJointAngles.col(1);
  std::vector<double> vec(abAdAngs.data(), abAdAngs.data() + abAdAngs.rows() * abAdAngs.cols()); // cast as std::vector
- double myMedian=gtsamutils::median(vec);
+ double myMedian=mathutils::median(vec);
  uint medIdx=gtsamutils::nearestIdxToVal(vec, myMedian);
  //std::cout<<std::endl<<"ab/ad rot median is "<<myMedian<<" and occurs at index "<<medIdx<<" (true val in array = "<<vec[medIdx]<<")"<<std::endl;
  R_B_to_B2 = lowerBodyPoseEstimator::getDistalImuOrientationAdjustmentGivenDesiredAbAdAngle(R_ProxSeg_to_N[medIdx],distalImuPose[medIdx].rotation(),0.0,rightAxisDist,distalImuToProxJointCtr,distalImuToDistalJointCtr); // R[B->B2]