I wish to get CMDeviceMotion information and plot the machine’s movement trajectory after information processing. (I referred to Mack Raymond’s article on implementing sensor information for Android units in Python) https://medium.com/analytics-vidhya/exploring-data-acquisition-and-trajectory-tracking-with-android-devices-and-python-9fdef38f25ee ）。 Now I wish to use OC and C++ code to implement functions on the cell finish.
I anticipate to acquire an array of Earth 3D coordinate vectors (earth_position_x, earth_position_y, earth_position_z) for the machine from the code above.
My query is after I use the 3D array to attract a visible chart, I get an irregular chart that has nothing to do with the trajectory of my machine’s motion. I wish to know what’s mistaken with my code, truly I do know it’s lack of noise deal with of earth_position vectors, however the outcomes I bought are fully mistaken, or anybody has different concepts to get machine trajectory from CMDeviceMotion information？
Thanks to your assist.
Get machine’s movement trajectory from CMDeviceMotion information.
I attempted to transform the physique body to the earth body coordinates by the CMDeviceMotion.perspective.rotationMatrix and quaternion attributes, however I could not acquire the proper information.
Right here is my code:
IMUTransfer *switch;
#outline kG (-9.81)
@interface MyController () {
float *sensorInfoForC;
}
@property (nonatomic, sturdy) CMMotionManager *supervisor;
@property (nonatomic,sturdy)__block NSMutableArray *sensorInfo;
@finish
- (void)starCollection {
self.supervisor = [[CMMotionManager alloc] init];
self.supervisor.deviceMotionUpdateInterval = 0.01;
self.supervisor.showsDeviceMovementDisplay = YES;
NSOperationQueue *queue = [[NSOperationQueue alloc] init];
queue.maxConcurrentOperationCount = 1;
if (self.supervisor.isDeviceMotionAvailable) {
[self.manager startDeviceMotionUpdatesToQueue:queue withHandler:^(CMDeviceMotion * _Nullable motion, NSError * _Nullable error) {
CMAcceleration userAcceleration = motion.userAcceleration;
CMAcceleration gravity = motion.gravity;
CMRotationRate rotationRate = motion.rotationRate;
CMAttitude *attitude = motion.attitude;
double roll = attitude.roll;
double pitch = attitude.pitch;
double yaw = attitude.yaw;
[_sensorInfo addObjectsFromArray:@[
@(userAcceleration.x*kG),@(userAcceleration.y*kG),@(userAcceleration.z*kG),
@(gravity.x*kG),@(gravity.y*kG),@(gravity.z*kG),
@(0),@(0),@(0),
@(pitch),@(roll),@(yaw)]
];
}];
}
}
- (void)stopCollection {
switch = new IMUTransfer();
if (self.supervisor.isDeviceMotionAvailable) {
[self.manager stopDeviceMotionUpdates];
}
NSDateFormatter *formatter = [[NSDateFormatter alloc] init];
[formatter setDateFormat:@"yyyy-MM-dd HH:mm:ss:SSS"];
NSString *dateString = [formatter stringFromDate:[NSDate date]];
NSString *fileName = [NSString stringWithFormat:@"iOS-%@",dateString];
NSArray *paths = NSSearchPathForDirectoriesInDomains(NSDocumentDirectory,NSUserDomainMask,YES);
NSString *fullName = [NSString stringWithFormat:@"%@.csv", fileName];
self.fileBasePath = [[paths objectAtIndex:0] stringByAppendingPathComponent:@"DeviceInfo"];
[self creatDir:self.fileBasePath];
self.fileFullPath = [self.fileBasePath stringByAppendingPathComponent:fullName];
[@"" writeToFile:self.fileFullPath atomically:YES encoding:NSUTF8StringEncoding error:nil];
sensorInfoForC = new float[_sensorInfo.count] ();
for (int i = 0; i < _sensorInfo.depend; i++) {
sensorInfoForC[i] = [_sensorInfo[i] floatValue];
}
transfer->save_csv_sensorInfo(sensorInfoForC,(int)_sensorInfo.depend,kDataItemCount,self.fileFullPath.UTF8String);
}
code in C++
/// Arrange rotation matrices
Mat R_x(float x) {
// physique body rotation about x axis
Mat Rx = Mat::zeros(3, 3, CV_32FC1);
Rx.at<float>(0, 0) = 1;
Rx.at<float>(1, 1) = cos(-x);
Rx.at<float>(1, 2) = -sin(-x);
Rx.at<float>(2, 1) = sin(-x);
Rx.at<float>(2, 2) = cos(-x);
return Rx;
}
Mat R_y(float y) {
// physique body rotation about y axis
Mat Ry = Mat::zeros(3, 3, CV_32FC1);
Ry.at<float>(0, 0) = cos(-y);
Ry.at<float>(0, 2) = -sin(-y);
Ry.at<float>(1, 1) = 1;
Ry.at<float>(2, 0) = sin(-y);
Ry.at<float>(2, 2) = cos(-y);
return Ry;
}
Mat R_z(float z) {
// physique body rotation about z axis
Mat Rz = Mat::zeros(3, 3, CV_32FC1);
Rz.at<float>(0, 0) = cos(-z);
Rz.at<float>(0, 1) = -sin(-z);
Rz.at<float>(1, 0) = sin(-z);
Rz.at<float>(1, 1) = cos(-z);
Rz.at<float>(2, 2) = 1;
return Rz;
}
float cumtrapz(const vector<float>& y, float dx) {
float integral = 0.0;
for (int i = 1; i < y.dimension(); i++) {
integral += (y[i] + y[i-1]) / 2.0 * dx;
}
return integral;
}
vector<float> double_integral(const vector<float>& a, float dt) {
vector<float> v(a.dimension());
for (int i = 0; i < a.dimension(); i++) {
if (i == 0) {
v[i] = a[i] * dt;
} else {
v[i] = (a[i] + a[i-1]) / 2.0 * dt;
}
}
vector<float> s(v.dimension());
s[0] = 0.0;
for (int i = 0; i < v.dimension(); i++) {
if (i == 0) {
s[i] = v[i] * dt;
} else {
s[i] = (v[i] + v[i-1]) / 2.0 * dt;
}
}
s[0] = s[1];
return s;
}
/// deal with sensor information and write to .csv file
void IMUTransfer::save_csv_sensorInfo(float *information, int dataArraySize ,int itemCount, const char *filePath) {
int data_rows = dataArraySize / itemCount;
Mat mat(data_rows, itemCount, CV_32FC1);
for (int row = 0; row < data_rows; row++) {
for (int col = 0; col < itemCount; col++) {
float temp = information[row * itemCount + col];
mat.at<float>(row,col) = temp;
}
}
dm_mat = mat;
printMat(dm_mat);
cv::Mat line = cv::Mat::zeros(3, data_rows, CV_32FC1);
for (int row = 0; row < 3; row++) {
for (int col = 0; col < data_rows; col++) {
line.at<float>(row,col) = dm_mat.at<float>(col,row);
}
}
Mat pitch = dm_mat.col(9).clone();
Mat roll = dm_mat.col(10).clone();
Mat yaw = dm_mat.col(11).clone();
// Carry out body transformations (physique body --> earth body)
Mat earth_linear = cv::Mat::zeros(3, data_rows, CV_32FC1);
for (int i = 0; i < data_rows; i++) {
earth_linear.col(i) = R_z(yaw.at<float>(i)) * R_y(roll.at<float>(i)) * R_x(pitch.at<float>(i)) * line.col(i);
}
vector<float> earth_inear_accelration_x(data_rows);
vector<float> earth_inear_accelration_y(data_rows);
vector<float> earth_inear_accelration_z(data_rows);
vector<float> earth_position_x(data_rows);
vector<float> earth_position_y(data_rows);
vector<float> earth_position_z(data_rows);
for (int row = 0; row < data_rows; row++) {
earth_inear_accelration_x[row] = earth_linear.at<float>(0,row);
earth_inear_accelration_y[row] = earth_linear.at<float>(1,row);
earth_inear_accelration_z[row] = earth_linear.at<float>(2,row);
}
// Double combine
dt = 0.01;
earth_position_x = double_integral(earth_inear_accelration_x, dt);
earth_position_y = double_integral(earth_inear_accelration_y, dt);
earth_position_z = double_integral(earth_inear_accelration_z, dt);
std::ofstream outfile(filePath, std::ios_base::trunc);
outfile << "userAcceleration.x" << ',' << "userAcceleration.y" << ',' << "userAcceleration.z" << ',' << "gravity.x" << ',' << "gravity.y" << ',' << "gravity.z" << ',' << "acceleration.x" << ',' << "acceleration.y" << ',' << "acceleration.z" << ',' << "rotationRate.x" << ',' << "rotationRate.y" << ',' << "rotationRate.z" << ',' << "earthPosition.x" << ',' << "earthPosition.y" << ',' << "earthPosition.z" << 'n';
for (int i = 0; i < data_rows; i++) {
for (int j = 0; j < itemCount; j++) {
outfile << information[i * itemCount + j] << ',';
}
outfile << earth_position_x[i] << ',' << earth_position_y[i] << ',' << earth_position_z[i] << ',';
outfile << 'n';
}
outfile.shut();
}
