Использование кватерниона вместо рулона, тангажа и рыскания для отслеживания движения устройства

Question

Пожалуйста, примите мой длинный вопрос, я стараюсь сделать его максимально ясным.

То, что я пытаюсь сделать, получить отношение (шаг качки и рыскание), когда снимок сделан с помощью камеры, а затем сохранить значения отношения к nsuserdefaults. После сохранения изменится ориентация, а затем попытайтесь привести телефон в такое же положение, что и изображение, сделанное путем постоянного сравнения значений ориентации (сохраненных и текущих).

В целях интерфейса пользовательский интерфейс имеет 3 точки (по одному для каждого параметра ориентации) на экране, которые направляют правильную ориентацию изображения. При достижении правильного положения на экране отображается флаг соответствия.

Я находил значения крена, тангажа и рыскания, как:

CMQuaternion quat = self.motionManager.deviceMotion.attitude.quaternion; 
myRoll = radiansToDegrees(atan2(2*(quat.y*quat.w - quat.x*quat.z), 1 - 2*quat.y*quat.y - 2*quat.z*quat.z)) ; 
myPitch = radiansToDegrees(atan2(2*(quat.x*quat.w + quat.y*quat.z), 1 - 2*quat.x*quat.x - 2*quat.z*quat.z)); 
myYaw = radiansToDegrees(2*(quat.x*quat.y + quat.w*quat.z)); 

Когда я заметил, что некоторые различия в значениях рыскания, я искал и нашел здесь: link, что

yaw, pitch and roll from a quaternion you will have the same problem as if you were using just yaw, pitch and roll. You have to use quaternions EVERYWHERE in your code and forget about yaw, pitch and roll

Так что теперь я думаю, что мне нужно все снова закодировать ... Пожалуйста, не могли бы вы так добросердечно указать мне пример кода для использования Quaternion для этой цели?

Вот код я работаю над:

В ViewController.m, в пределах изображения: didFinishSavingWithError:

[self.motionManager startDeviceMotionUpdatesToQueue:[NSOperationQueue mainQueue] withHandler:^(CMDeviceMotion *motion, NSError *error) { 

     CMQuaternion quat = self.motionManager.deviceMotion.attitude.quaternion; 
     double tempYaw = radiansToDegrees(asin(2*(quat.x*quat.y + quat.w*quat.z))); 
     double tempRoll = radiansToDegrees(atan2(2*(quat.y*quat.w - quat.x*quat.z), 1 - 2*quat.y*quat.y - 2*quat.z*quat.z)) ; 
     double tempPitch = radiansToDegrees(atan2(2*(quat.x*quat.w + quat.y*quat.z), 1 - 2*quat.x*quat.x - 2*quat.z*quat.z)); 

     if (savingGyroOrientation == YES) { 

      NSLog(@"Roll = %f degrees",tempRoll); 
      NSLog(@"Pitch = %f degrees",tempPitch); 
      NSLog(@"Yaw = %f degrees",tempYaw); 

      [self.deviceStatus setDouble:tempRoll forKey:@"DeviceRoll"]; 
      [self.deviceStatus setDouble:tempPitch forKey:@"DevicePitch"]; 
      [self.deviceStatus setDouble:tempYaw forKey:@"DeviceYaw"]; 
      [self.deviceStatus synchronize]; 

      savingGyroOrientation = NO; 
      checkingGyroOrientation = YES; 
      self.savingLabel.hidden = YES; 
      self.startTimerButton.hidden = NO; 

     } 
     savingGyroOrientation = NO; 
     checkingGyroOrientation = YES; 
     self.savingLabel.hidden = YES; 
     self.startTimerButton.hidden = NO; 
    } 

    if (timerRunning == YES) { 
     if (checkingGyroOrientation == YES) { 

      self.takePicButton.hidden = YES; 

      int xRoll, yPitch, xYaw, yYaw; 
      // Roll Checking 
      if (tempRoll >= [self.deviceStatus doubleForKey:@"DeviceRoll"]-1 && tempRoll <= [self.deviceStatus doubleForKey:@"DeviceRoll"]+1) { 

       [self.rollDot setFrame:CGRectMake(150, 195, 20, 20)]; 
       self.rollToR.hidden = YES; 
       self.rollToL.hidden = YES; 
       self.rollDot.hidden = NO; 
       rollOk = YES; 
      }else{ 
       rollOk = NO; 
       self.rollDot.hidden = YES; 
       self.rollToR.hidden = NO; 
       self.rollToL.hidden = NO; 

       if (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"] < 0) { 
        xRoll = 150 + (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"]); 
        self.rollToR.hidden = YES; 
        if (xRoll <= 0) { 
         [self.rollToL setFrame:CGRectMake(0, 195, 20, 20)]; 
        }else if (xRoll>= 300){ 
         [self.rollToL setFrame:CGRectMake(300, 195, 20, 20)]; 
        }else{ 
         [self.rollToL setFrame:CGRectMake(xRoll, 195, 20, 20)]; 
        } 
       } 
       if (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"] > 0){ 
        xRoll = 150 + (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"]); 
        self.rollToL.hidden = YES; 
        if (xRoll <= 0) { 
         [self.rollToR setFrame:CGRectMake(0, 195, 20, 20)]; 
        }else if (xRoll>=300){ 
         [self.rollToR setFrame:CGRectMake(300, 195, 20, 20)]; 
        }else{ 
         [self.rollToR setFrame:CGRectMake(xRoll, 195, 20, 20)]; 
        } 
       } 
       if (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"] > 180){ 
        xRoll = 150 + (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"]-360); 
        self.rollToR.hidden = YES; 
        if (xRoll <= 0) { 
         [self.rollToL setFrame:CGRectMake(0, 195, 20, 20)]; 
        }else if (xRoll>=300){ 
         [self.rollToL setFrame:CGRectMake(300, 195, 20, 20)]; 
        }else{ 
         [self.rollToL setFrame:CGRectMake(xRoll, 195, 20, 20)]; 
        } 
       } 
       if (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"] < -180){ 
        xRoll = 150 + (tempRoll - [self.deviceStatus doubleForKey:@"DeviceRoll"]+360); 
        self.rollToL.hidden = YES; 
        if (xRoll <= 0) { 
         [self.rollToR setFrame:CGRectMake(0, 195, 20, 20)]; 
        }else if (xRoll >= 300){ 
         [self.rollToR setFrame:CGRectMake(300, 195, 20, 20)]; 
        }else{ 
         [self.rollToR setFrame:CGRectMake(xRoll, 195, 20, 20)]; 
        } 
       } 
      } 
      //Pitch Checking 
      if (tempPitch >= [self.deviceStatus doubleForKey:@"DevicePitch"]-1 && tempPitch <= [self.deviceStatus doubleForKey:@"DevicePitch"]+1) { 
       [self.pitchDot setFrame:CGRectMake(150, 195, 20, 20)]; 
       self.pitchDot.hidden = NO; 
       self.pitchToDown.hidden = YES; 
       self.pitchToUp.hidden = YES; 
       pitchOk = YES; 
      }else{ 
       pitchOk = NO; 
       self.pitchDot.hidden = YES; 
       self.pitchToDown.hidden = NO; 
       self.pitchToUp.hidden = NO; 
       if (tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"] < 0) { 
        yPitch = 195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"]); 
        //       NSLog(@"tempPitch is %0.02f Difference is %0.02f, yPitch is %0.02f",tempPitch, tempPitch-[self.deviceStatus doubleForKey:@"DevicePitch"],195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"])); 
        self.pitchToDown.hidden = YES; 
        if (yPitch <= 0) { 
         [self.pitchToUp setFrame:CGRectMake(150, 0, 20, 20)]; 
        }else if (yPitch >= 390) { 
         [self.pitchToUp setFrame:CGRectMake(150, 390, 20, 20)]; 
        }else{ 
         [self.pitchToUp setFrame:CGRectMake(150, yPitch, 20, 20)]; 
        } 
       } 
       if (tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"] > 0){ 
        yPitch = 195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"]); 
        //       NSLog(@"tempPitch is %0.02f Difference is %0.02f, yPitch is %0.02f",tempPitch, tempPitch-[self.deviceStatus doubleForKey:@"DevicePitch"],195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"])); 
        self.pitchToUp.hidden = YES; 
        if (yPitch <= 0) { 
         [self.pitchToDown setFrame:CGRectMake(150, 0, 20, 20)]; 
        }else if (yPitch >= 390) { 
         [self.pitchToDown setFrame:CGRectMake(150, 390, 20, 20)]; 
        }else{ 
         [self.pitchToDown setFrame:CGRectMake(150, yPitch, 20, 20)]; 
        } 
       } 
       if (tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"] < -180){ 
        yPitch = 195+tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"] + 360; 
        //       NSLog(@"tempPitch is %0.02f Difference is %0.02f, yPitch is %0.02f",tempPitch, tempPitch-[self.deviceStatus doubleForKey:@"DevicePitch"],195+(tempPitch - [self.deviceStatus doubleForKey:@"DevicePitch"])); 
        //       NSLog(@"*yPitch is %d",yPitch); 
        self.pitchToUp.hidden = YES; 
        self.pitchToDown.hidden = NO; 
        if (yPitch <= 0) { 
         [self.pitchToDown setFrame:CGRectMake(150, 0, 20, 20)]; 
        }else if (yPitch >= 390) { 
         [self.pitchToDown setFrame:CGRectMake(150, 390, 20, 20)]; 
        }else{ 
         [self.pitchToDown setFrame:CGRectMake(150, yPitch, 20, 20)]; 
        } 
       } 
      } 
      if (tempYaw >= [self.deviceStatus doubleForKey:@"DeviceYaw"]-2 && tempYaw <= [self.deviceStatus doubleForKey:@"DeviceYaw"]+2) { 

       [self.yawDot setFrame:CGRectMake(150, 195, 20, 20)]; 
       self.yawDot.hidden = NO; 
       self.rotateRight.hidden = YES; 
       self.rotateLeft.hidden = YES; 
       yawOk = YES; 
      }else{ 
       yawOk = NO; 
       self.yawDot.hidden = YES; 
       self.rotateRight.hidden = NO; 
       self.rotateLeft.hidden = NO; 

       if (tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"] < 0) { 
        xYaw = 150+(tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"]); 
        yYaw = 195-1.3*(tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"]); 
        NSLog(@"current yaw is %0.02f Difference is %0.02f",tempYaw, tempYaw-[self.deviceStatus doubleForKey:@"DeviceYaw"]); 
        NSLog(@"saved Yaw is %0.02f",[self.deviceStatus doubleForKey:@"DeviceYaw"]); 
        NSLog(@"xYaw is %d, yYaw is %d",xYaw,yYaw); 
        self.rotateRight.hidden = YES; 
        if (xYaw <=0 && yYaw >=390) { 
         [self.rotateLeft setFrame:CGRectMake(0, 390, 20, 20)]; 
        }else{ 
         [self.rotateLeft setFrame:CGRectMake(xYaw, yYaw, 20, 20)]; 
        } 

       }if (tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"] > 0){ 
        xYaw = 150+(tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"]); 
        yYaw = 195-1.3*(tempYaw - [self.deviceStatus doubleForKey:@"DeviceYaw"]); 
        NSLog(@"current yaw is %0.02f Difference is %0.02f",tempYaw, tempYaw-[self.deviceStatus doubleForKey:@"DeviceYaw"]); 
        NSLog(@"saved Yaw is %0.02f",[self.deviceStatus doubleForKey:@"DeviceYaw"]); 
        NSLog(@"*xYaw is %d, yYaw is %d",xYaw,yYaw); 
        self.rotateLeft.hidden = YES; 
        if (xYaw >=300 && yYaw <=0) { 
         [self.rotateRight setFrame:CGRectMake(300, 0, 20, 20)]; 
        }else{ 
         [self.rotateRight setFrame:CGRectMake(xYaw, yYaw, 20, 20)]; 
        } 
       } 
      } 

      if (rollOk == YES && pitchOk == YES && yawOk ==YES) { 
       self.orientationOkay.hidden = NO; 
       self.centerCircle.hidden = YES; 
       self.rollDot.hidden = YES; 
       self.pitchDot .hidden =YES; 
       self.yawDot.hidden = YES; 
       [self.clickTimer invalidate]; 
       self.clickTimer = nil; 
       self.takePicButton.hidden = NO; 
       timerRunning = NO; 
       [self.motionManager stopDeviceMotionUpdates]; 
       [self.deviceStatus removeObjectForKey:@"DeviceRoll"]; 
       [self.deviceStatus removeObjectForKey:@"DevicePitch"]; 
       [self.deviceStatus removeObjectForKey:@"DeviceYaw"]; 
       [self.deviceStatus removeObjectForKey:@"DeviceAngle"]; 

      }else{ 

       self.orientationOkay.hidden = YES; 
       if (flagger == YES) { 
        self.centerCircle.hidden = NO; 
       } 
       else{ 
        self.centerCircle.hidden = YES; 
       } 
      } 
     } 
    }else{ 
     self.rotateRight.hidden = YES; 
     self.rotateLeft.hidden = YES; 
     self.rollToL.hidden = YES; 
     self.rollToR.hidden = YES; 
     self.pitchToDown.hidden = YES; 
     self.pitchToUp.hidden = YES; 
     self.rollDot.hidden = NO; 
     self.pitchDot .hidden =NO; 
     self.yawDot.hidden = NO; 
     [self.yawDot setFrame:CGRectMake(0, 390, 20, 20)]; 
     [self.rollDot setFrame:CGRectMake(0, 195, 20, 20)]; 
     [self.pitchDot setFrame:CGRectMake(150, 0, 20, 20)]; 

    } 
    }]; 

Пожалуйста, дайте мне знать, если какие-либо дополнительные детали необходимы на этом.

Любые предложения или советы всегда приветствуются, :) Я - программист для программирования и для ios.

Спасибо!

Answer 1

Я думаю, что следующие вещи необходимы для управления к задаче:

1. Прежде всего, вам необходимо хорошее понимание кватернионов (пропустить это, если вы уже подружились с ними). Я рекомендую OpenGL:Tutorials:Using Quaternions to represent rotation или The Matrix and Quaternions FAQ. Это позволяет иметь в виду, что (x, y, z) представляют ось для вращения вокруг (не нормированной) и w = cos (альфа/2), то есть приблизительно для количества вращения.

2. Как CMQuaternion - это просто структура, поэтому выполнить все вычисления сложно. Используйте полный функциональный класс кватернионов, а не cocoamath (вам нужно, по крайней мере, Quaternion.h, .m и QuaternionOperation.m от trunk).

3. В настоящее время основные соображения:

   1. Разница (или иногда указано как деление) между двумя кватернионов определяется как угловое смещение от одной ориентации к другой может быть путь.Она определяется как
      D = A ^-1 * б
      Таким образом, это выражается дельта из текущего положения в конечное положение.

   2. Имея эту дельту, вам необходимо определить условия, которые необходимо выполнить для рассмотрения целевой ориентации по достижении. Моя первая идея - использовать треугольник. Это может быть легко извлечен из ш компонента выше рассчитанного г кватернион по:
      альфа = 2 * агссоз (ш)
      Область агссоз ограничена, но это не должно быть проблемой в этом случае, как мы особенно заинтересованы в небольших значениях.

Может быть, стоит подчеркнуть, что каждый 3D вращение имеет два единичных кватернионов представления, д и -q. Это может быть запутанным, но неважно.

---

Update: Так немного псевдокоде будет выглядеть примерно так:

CMQuaternion cmQ = attitude.quaternion; 
// Get an instance of cocoamath's Quaternion for our currently reported quaternion 
Quaternion current = [Quaternion initWithRe:(double)cmQ.w i:(double)cmQ.x j:(double)cmQ.y k:(double)cmQ.z]; 
// the complex conjugate and normalised to be on the safe side 
Quaternion inverse = [current inverse]; 
// build the delta, assuming you have your stored direction as class member targetQuaternion 
Quaternion diff = [inverse multiply:targetQuaternion]; 
float alpha = 2 * acos (diff.Re); 
// maxDeltaAngle is your class member variable defining the angle from which you assume the position as restored (radians) 
if (fabs (alpha) < maxDeltaAngle) { 
    // do my fancy camera things 
} 

Да, это не то, что тривиальный в самом начале. Как сказал Али в своем ответе, визуализация является важной проблемой. Мое решение просто решает исходную математическую часть.

Answer 2

Я не говорю, что следующее решение вашей проблемы, я понятия не имею, насколько это удобно, но я считаю, что это стоит того. Я вижу два вопроса: как вы храните отношение и как вы их визуализируете.

Хранение. Я бы сохранил отношение либо как кватернион, либо как матрицу вращения; CMAttitude обеспечивает оба. (Я лично предпочитаю матрицы вращения для кватернионов, так как, по моему мнению, матрицы поворота легче понять. Это только личное предпочтение.)

Визуализация. Вы используете 3 маркера, чтобы привести телефон в такое же положение, как и сохраненное. Эти 3 маркера пришли от рыскания, подачи и броска, что фактически разрушило стабильность вашего приложения. Вместо этих трех маркеров я бы визуализировал поворот между сохраненным отношением и текущим. Один из способов сделать это - проецировать повернутые 3 базовые векторы на экран телефона. Хорошая вещь о матрицах вращения заключается в том, что они дают вам именно это без каких-либо дополнительных вычислений. Например, чтобы визуализировать вращение на

| 1 0 0 | 
| 0 1 0 | 
| 0 0 1 | 

эта матрица вращения представляет, я только нужно построить 3 стрелы из [0, 0, 0] к (I) [1, 0, 0], (II) [0, 1, 0] и (III) [0, 0, 1]. То есть мне нужно всего лишь построить строки или столбцы матрицы (также зависит от того, что вы хотите, лучше ли строки или столбцы).

Чтобы получить вращение между сохраненной матрицей S вращения и текущей матрицей вращения C вам нужно вычислить C ^T S (прописью: C транспонировать раз S). Вы выровняли телефон с сохраненным отношением, если ваша матрица вращения указана выше.

Отличный учебник по матрицам вращения - это рукопись Direction Cosine Matrix IMU: Theory.

Другой способ визуализации вращения между сохраненным и текущим отношением - преобразовать его в форму угловой оси. Затем я проецирую ось на экран телефона. Сначала пользователь выравнивает телефон с осью, а затем поворачивается вокруг оси в соответствии с сохраненным отношением. Кватернионы в основном представляют собой ось-угловую форму, хотя и нетривиальным образом.

Для изменения этой идеи требуется существенная дальнейшая работа в соответствии с вашими потребностями; это, безусловно, не полный ответ. Тем не менее, я надеюсь, что это поможет.