Skip to content
Snippets Groups Projects
Commit f5871253 authored by Markus Grotz's avatar Markus Grotz
Browse files

added CMakeLists.txt for LibIMU + removed newmat dependency

parent 324151e1
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
source/RobotAPI/libraries/drivers/XsensIMU/CMakeLists.txt
+ 2
− 10
View file @ f5871253
armarx_component_set_name("XsensIMU")
#find_package(MyLib QUIET)
#armarx_build_if(MyLib_FOUND "MyLib not available")
#
# all include_directories must be guarded by if(Xyz_FOUND)
# for multiple libraries write: if(X_FOUND AND Y_FOUND)....
#if(MyLib_FOUND)
# include_directories(${MyLib_INCLUDE_DIRS})
#endif()
set(COMPONENT_LIBS ArmarXCore RobotAPIUnits ArmarXCoreObservers)
set(COMPONENT_LIBS ArmarXCore RobotAPIUnits ArmarXCoreObservers LibIMU)
set(SOURCES
./XsensIMU.cpp
......@@ -23,3 +14,4 @@ set(HEADERS
armarx_add_component("${SOURCES}" "${HEADERS}")
add_subdirectory(IMU)
source/RobotAPI/libraries/drivers/XsensIMU/IMU/CMakeLists.txt 0 → 100644
+ 32
− 0
View file @ f5871253
armarx_set_target("LibIMU")
set(LIB_NAME LibIMU)
set(LIB_VERSION 0.0.1)
set(LIB_SOVERSION 1)
set(LIBS pthread)
set(LIB_FILES
./Xsens/XsensMTiModule.cpp
./IIMUEventDispatcher.cpp
./IMUDeducedReckoning.cpp
./IMUDevice.cpp
./IMUHelpers.cpp
./IMUEvent.cpp
./IMUState.cpp
)
set(LIB_HEADERS
./Xsens/Xsens.h
./Xsens/XsensMTiModule.h
./IIMUEventDispatcher.h
./IMUDeducedReckoning.h
./IMUDevice.h
./IMUEvent.h
./IMUHelpers.h
./IMU.h
./IMUState.h
./Includes.h
)
armarx_add_library("${LIB_NAME}" "${LIB_VERSION}" "${LIB_SOVERSION}" "${LIB_FILES}" "${LIB_HEADERS}" "${LIBS}")
source/RobotAPI/libraries/drivers/XsensIMU/IMU/IMUHelpers.h
+ 0
− 193
View file @ f5871253
......@@ -70,200 +70,7 @@ namespace IMU
}
};
/*
function [Qavg]=avg_quaternion_markley(Q)
% Form the symmetric accumulator matrix
A=zeros(4,4);
M=size(Q,1);
for i=1:M
q=Q(i,:)';
A=q*q'+A; % rank 1 update
end
% scale
A=(1.0/M)*A;
% Get the eigenvector corresponding to largest eigen value
[Qavg, Eval]=eigs(A,1);
end
*/
class CQuaternion
{
public:
static void Test(const int N, const int M, const float AzimuthalA, const float AzimuthalB, const float PolarA, const float PolarB, const float AlphaA, const float AlphaB)
{
const float AzimuthalMax = std::max(AzimuthalA,AzimuthalB);
const float AzimuthalMin = std::min(AzimuthalA,AzimuthalB);
const float AzimuthalRange = AzimuthalMax - AzimuthalMin;
const float PolarMax = std::max(PolarA,PolarB);
const float PolarMin = std::min(PolarA,PolarB);
const float PolarRange = PolarMax - PolarMin;
const float AlphaMax = std::max(AlphaA,AlphaB);
const float AlphaMin = std::min(AlphaA,AlphaB);
const float AlphaRange = AlphaMax - AlphaMin;
const int Total = N * M;
CQuaternion* pQuaternions = new CQuaternion[Total];
float Axis[3] = { 0.0f , 0.0f , 0.0f };
for(int i = 0 , k = 0 ; i < N ; ++i)
{
const float Azimuthal = (AzimuthalRange * (float(rand()) / float(RAND_MAX))) + AzimuthalMin;
const float CosAzimuthal = std::cos(Azimuthal);
const float SinAzimuthal = std::sin(Azimuthal);
const float Polar = (PolarRange * (float(rand()) / float(RAND_MAX))) + PolarMin;
const float CosPolar = std::cos(Polar);
const float SinPolar = std::sin(Polar);
Axis[0] = CosAzimuthal * SinPolar;
Axis[1] = SinAzimuthal * SinPolar;
Axis[2] = CosPolar;
for(int j = 0 ; j < M ; ++j)
pQuaternions[k].Set(Axis,AlphaRange * float(rand()) / float(RAND_MAX) + AlphaMin);
}
CQuaternion Mean = MeanNQuaternion(pQuaternions,Total);
Mean.MakeItUnitary();
float Alpha = 0.0f;
Mean.Get(Axis,Alpha);
delete[] pQuaternions;
}
static CQuaternion MeanNQuaternion(const CQuaternion* pQuaternions, const int N)
{
float A[4][4];
memset(A,0,sizeof(float) * 16);
for(int i = 0 ; i < N ; ++i)
for(int r = 0 ; r < 4 ; ++r)
for(int c = 0 ; c < 4 ; ++c)
A[r][c] += pQuaternions[i][r] * pQuaternions[i][c];
NEWMAT::Matrix Covariance(4,4);
for(int r = 0 ; r < 4 ; ++r)
for(int c = 0 ; c < 4 ; ++c)
Covariance(r + 1,c + 1) = A[r][c] / float(N);
DisplayMatrix(Covariance);
NEWMAT::DiagonalMatrix D;
NEWMAT::Matrix U;
NEWMAT::Matrix V;
NEWMAT::SVD(Covariance,D,U,V);
DisplayMatrix(Covariance);
DisplayMatrix(D);
DisplayMatrix(U);
DisplayMatrix(V);
return CQuaternion(float(U(1,1)),float(U(1,2)),float(U(1,3)),float(U(1,4)));
}
static void DisplayMatrix(NEWMAT::Matrix& M)
{
const int Rows = M.Nrows();
const int Cols = M.Ncols();
std::cout << std::endl;
for(int r = 1 ; r <= Rows ; ++r)
{
std::cout << "|\t";
for(int c = 1 ; c <= Cols ; ++c)
std::cout << "\t" << M(r,c) << "\t";
std::cout << "|" << std::endl;
}
std::cout << std::endl;
}
static void DisplayMatrix(NEWMAT::DiagonalMatrix& M)
{
for(int r = 1 ; r <= M.Nrows() ; ++r)
{
std::cout << "|\t";
for(int c = 1 ; c < M.Ncols() ; ++c)
if (r == c)
std::cout << M(r,c) << "\t";
else
std::cout << "0.0\t";
std::cout << "|" << std::endl;
}
}
CQuaternion()
{
memset(m_Elements,0,sizeof(float) * 4);
}
CQuaternion(const float Axis[3], const float Angle)
{
const float Sin = std::sin(Angle * 0.5f);
m_Elements[0] = std::cos(Angle * 0.5f);
m_Elements[1] = Sin * Axis[0];
m_Elements[2] = Sin * Axis[1];
m_Elements[3] = Sin * Axis[2];
}
CQuaternion(const float w, const float i, const float j, const float k)
{
m_Elements[0] = w;
m_Elements[1] = i;
m_Elements[2] = j;
m_Elements[3] = k;
}
void Set(const float Axis[3], const float Angle)
{
const float Sin = std::sin(Angle * 0.5f);
m_Elements[0] = std::cos(Angle * 0.5f);
m_Elements[1] = Sin * Axis[0];
m_Elements[2] = Sin * Axis[1];
m_Elements[3] = Sin * Axis[2];
}
void Get(float* pAxis, float& Angle)
{
const float Normalization = std::sqrt(1.0f - m_Elements[0] * m_Elements[0]);
Angle = std::acos(m_Elements[0]) * 2.0f;
pAxis[0] = m_Elements[1] / Normalization;
pAxis[1] = m_Elements[2] / Normalization;
pAxis[2] = m_Elements[3] / Normalization;
}
void Conjugate()
{
m_Elements[1] *= -1.0f;
m_Elements[2] *= -1.0f;
m_Elements[3] *= -1.0f;
}
void Reciprocal()
{
const float SquareNegativeNormalization = -(m_Elements[0] * m_Elements[0] + m_Elements[1] * m_Elements[1] + m_Elements[2] * m_Elements[2] + m_Elements[3] * m_Elements[3]);
m_Elements[0] /= -SquareNegativeNormalization;
m_Elements[1] /= SquareNegativeNormalization;
m_Elements[2] /= SquareNegativeNormalization;
m_Elements[3] /= SquareNegativeNormalization;
}
float Norm()
{
return std::sqrt(m_Elements[0] * m_Elements[0] + m_Elements[1] * m_Elements[1] + m_Elements[2] * m_Elements[2] + m_Elements[3] * m_Elements[3]);
}
void MakeItUnitary()
{
const float Norm = std::sqrt(m_Elements[0] * m_Elements[0] + m_Elements[1] * m_Elements[1] + m_Elements[2] * m_Elements[2] + m_Elements[3] * m_Elements[3]);
m_Elements[0] /= Norm;
m_Elements[1] /= Norm;
m_Elements[2] /= Norm;
m_Elements[3] /= Norm;
}
inline float& operator[](const int i)
{
return m_Elements[i];
}
inline float operator[](const int i) const
{
return m_Elements[i];
}
protected:
float m_Elements[4];
};
}
......
source/RobotAPI/libraries/drivers/XsensIMU/IMU/Includes.h
+ 0
− 1
View file @ f5871253
......@@ -42,7 +42,6 @@
//LINUX INCLUDES
/////////////////////////////////////////////////////////////////////////////
#include <sys/time.h>
#include <newmat/newmatap.h>
#include <sched.h>
#include <errno.h>
/////////////////////////////////////////////////////////////////////////////
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment