Skip to content

Commit

Permalink
Merge pull request tmcg0#41 from tmcg0/remove-unused-functions
Browse files Browse the repository at this point in the history 
Remove unused functions
  • Loading branch information
@tmcg0
tmcg0 authored Dec 6, 2023
2 parents 6e3bd6f + 80c3b29 commit 2a67829
Show file tree
Hide file tree
Showing 3 changed files with 3 additions and 104 deletions.
11 changes: 0 additions & 11 deletions include/gtsamutils.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,16 +22,6 @@ namespace gtsamutils{
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>
Expand All @@ -40,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"));
Expand Down
92 changes: 1 addition & 91 deletions src/gtsamutils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand All @@ -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);
Expand Down
4 changes: 2 additions & 2 deletions src/lowerBodyPoseEstimator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand All @@ -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]
Expand Down

0 comments on commit 2a67829

Please sign in to comment.