c++ – get machine’s movement trajectory from CMDeviceMotion information

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();
}