I want to get CMDeviceMotion data and plot the device's motion trajectory after data processing. (I referred to Mack Raymond's article on implementing sensor data for Android devices in Python) https://medium.com/analytics-vidhya/exploring-data-acquisition-and-trajectory-tracking-with-android-devices-and-python-9fdef38f25ee ）。 Now I want to use OC and C++ code to implement applications on the mobile end.
I expect to obtain an array of Earth 3D coordinate vectors (earth_position_x, earth_position_y, earth_position_z) for the device from the code above.
My question is when I use the 3D array to draw a visual chart, I get an irregular chart that has nothing to do with the trajectory of my device's movement. I want to know what's wrong with my code, actually I know it is lack of noise handle of earth_position vectors, but the results I got are completely wrong, or anyone has other ideas to get device trajectory from CMDeviceMotion data？
Thanks for your help.
Get device's motion trajectory from CMDeviceMotion data.
I tried to convert the body frame to the earth frame coordinates through the CMDeviceMotion.attitude.rotationMatrix and quaternion attributes, but I couldn't obtain the correct data.
Here is my code:
IMUTransfer *transfer;
#define kG (-9.81)
@interface MyController () {
float *sensorInfoForC;
}
@property (nonatomic, strong) CMMotionManager *manager;
@property (nonatomic,strong)__block NSMutableArray *sensorInfo;
@end
- (void)starCollection {
self.manager = [[CMMotionManager alloc] init];
self.manager.deviceMotionUpdateInterval = 0.01;
self.manager.showsDeviceMovementDisplay = YES;
NSOperationQueue *queue = [[NSOperationQueue alloc] init];
queue.maxConcurrentOperationCount = 1;
if (self.manager.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 {
transfer = new IMUTransfer();
if (self.manager.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.count; i++) {
sensorInfoForC[i] = [_sensorInfo[i] floatValue];
}
transfer->save_csv_sensorInfo(sensorInfoForC,(int)_sensorInfo.count,kDataItemCount,self.fileFullPath.UTF8String);
}
code in C++
/// Set up rotation matrices
Mat R_x(float x) {
// body frame 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) {
// body frame 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) {
// body frame 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.size(); 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.size());
for (int i = 0; i < a.size(); i++) {
if (i == 0) {
v[i] = a[i] * dt;
} else {
v[i] = (a[i] + a[i-1]) / 2.0 * dt;
}
}
vector<float> s(v.size());
s[0] = 0.0;
for (int i = 0; i < v.size(); 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;
}
/// handle sensor data and write to .csv file
void IMUTransfer::save_csv_sensorInfo(float *data, 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 = data[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();
// Perform frame transformations (body frame --> earth frame)
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 integrate
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 << data[i * itemCount + j] << ',';
}
outfile << earth_position_x[i] << ',' << earth_position_y[i] << ',' << earth_position_z[i] << ',';
outfile << '\n';
}
outfile.close();
}
