Remove unused functions

include/gtsamutils.h

@@ -1,4 +1,4 @@
 // -------------------------------------------------------------------- //
 //    (c) Copyright 2021 Massachusetts Institute of Technology          //
 //    Author: Tim McGrath                                               //
 //    All rights reserved. See LICENSE file for license information.    //
@@ -8,7 +8,7 @@
 
 #pragma once
 
 #include <vector>
 #include <gtsam/geometry/Point3.h>
 #include <gtsam/geometry/Rot3.h>
 #include <gtsam/geometry/Pose3.h>
@@ -17,21 +17,11 @@
 #include "imu/imu.h"
 #include <gtsam/base/numericalDerivative.h>
 
 namespace gtsamutils{
     Eigen::MatrixXd Point3VectorToEigenMatrix(const std::vector<gtsam::Point3>& p);
     Eigen::MatrixXd Vector3VectorToEigenMatrix(const std::vector<gtsam::Vector3>& v);
     std::vector<gtsam::Rot3> Pose3VectorToRot3Vector(const std::vector<gtsam::Pose3>& poses);
     std::vector<gtsam::Point3> Pose3VectorToPoint3Vector(const std::vector<gtsam::Pose3>& poses);
-    std::vector<double> vectorizePoint3x(std::vector<gtsam::Point3>);
-    std::vector<double> vectorizePoint3y(std::vector<gtsam::Point3>);
-    std::vector<double> vectorizePoint3z(std::vector<gtsam::Point3>);
-    std::vector<double> vectorizeVector3X(std::vector<gtsam::Vector3>);
-    std::vector<double> vectorizeVector3Y(std::vector<gtsam::Vector3>);
-    std::vector<double> vectorizeVector3Z(std::vector<gtsam::Vector3>);
-    std::vector<double> vectorizeQuaternionS(std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d>>);
-    std::vector<double> vectorizeQuaternionX(std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d>>);
-    std::vector<double> vectorizeQuaternionY(std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d>>);
-    std::vector<double> vectorizeQuaternionZ(std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d>>);
     std::vector<double> vectorSetMagnitudes(const std::vector<Eigen::Vector3d>& v);
     Eigen::MatrixXd vectorRot3ToYprMatrix(const std::vector<gtsam::Rot3>& R);
     std::vector<gtsam::Rot3> imuOrientation(const imu& myImu); // pull out stored quaternion as a vector<Rot3>
@@ -40,7 +30,6 @@
     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"));

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.
@@ -189,76 +169,6 @@ namespace gtsamutils{
         return nearestIdx;
     }
 
-    std::vector<double> vectorizePoint3x(std::vector<gtsam::Point3> p){
-        std::vector<double> x(p.size());
-        for(uint i=0; i<p.size(); i++){
-            x[i]=p[i].x();
-        }
-        return x;
-    }
-    std::vector<double> vectorizePoint3y(std::vector<gtsam::Point3> p){
-        std::vector<double> y(p.size());
-        for(uint i=0; i<p.size(); i++){
-            y[i]=p[i].y();
-        }
-        return y;
-    }
-    std::vector<double> vectorizePoint3z(std::vector<gtsam::Point3> p){
-        std::vector<double> z(p.size());
-        for(uint i=0; i<p.size(); i++){
-            z[i]=p[i].z();
-        }
-        return z;
-    }
-    std::vector<double> vectorizeVector3X(std::vector<gtsam::Vector3> v){
-        std::vector<double> a(v.size());
-        for(uint i=0; i<v.size(); i++){
-            a[i]=v[i](0);
-        }
-        return a;
-    }
-    std::vector<double> vectorizeVector3Y(std::vector<gtsam::Vector3> v){
-        std::vector<double> a(v.size());
-        for(uint i=0; i<v.size(); i++){
-            a[i]=v[i](1);
-        }
-        return a;
-    }
-    std::vector<double> vectorizeVector3Z(std::vector<gtsam::Vector3> v){
-        std::vector<double> a(v.size());
-        for(uint i=0; i<v.size(); i++){
-            a[i]=v[i](2);
-        }
-        return a;
-    }
-    std::vector<double> vectorizeQuaternionS(std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d>> q){
-        std::vector<double> a(q.size());
-        for(uint i=0; i<q.size(); i++){
-            a[i]=q[i](0);
-        }
-        return a;
-    }
-    std::vector<double> vectorizeQuaternionX(std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d>> q){
-        std::vector<double> a(q.size());
-        for(uint i=0; i<q.size(); i++){
-            a[i]=q[i](1);
-        }
-        return a;
-    }
-    std::vector<double> vectorizeQuaternionY(std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d>> q){
-        std::vector<double> a(q.size());
-        for(uint i=0; i<q.size(); i++){
-            a[i]=q[i](2);
-        }
-        return a;
-    }
-    std::vector<double> vectorizeQuaternionZ(std::vector<Eigen::Vector4d,Eigen::aligned_allocator<Eigen::Vector4d>> q){
-        std::vector<double> a(q.size());
-        for(uint i=0; i<q.size(); i++){
-            a[i]=q[i](3);
-        }
-        return a;
-    }
     void saveMatrixToFile(const gtsam::Matrix& A, const std::string &s, const std::string& filename) {
         std::fstream stream(filename.c_str(), std::fstream::out | std::fstream::app);
         print(A, s + "=", stream);

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]