Undid Logmap refactor:
there is some subtle bug with that lambda that I could not resolve withing reasonable time.release/4.3a0
							parent
							
								
									dd6b70e08c
								
							
						
					
					
						commit
						c4b2cc5b0f
					
				|  | @ -317,26 +317,46 @@ Vector3 SO3::Logmap(const SO3& Q, ChartJacobian H) { | |||
| 
 | ||||
|   // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc.
 | ||||
|   // we do something special
 | ||||
|   if (tr + 1.0 < so3::kNearPiThresholdSq) { | ||||
|     // Used below
 | ||||
|     auto omegaFromQ = [](double W, double r, double w1, double w2, double w3) { | ||||
|   if (tr + 1.0 < 1e-3) { | ||||
|     if (R33 > R22 && R33 > R11) { | ||||
|       // R33 is the largest diagonal, a=3, b=1, c=2
 | ||||
|       const double W = R21 - R12; | ||||
|       const double Q1 = 2.0 + 2.0 * R33; | ||||
|       const double Q2 = R31 + R13; | ||||
|       const double Q3 = R23 + R32; | ||||
|       const double r = sqrt(Q1); | ||||
|       const double one_over_r = 1 / r; | ||||
|       const double norm = sqrt(w1 * w1 + w2 * w2 + w3 * w3 + W * W); | ||||
|       const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W); | ||||
|       const double sgn_w = W < 0 ? -1.0 : 1.0; | ||||
|       const double mag = M_PI - (2 * sgn_w * W) / norm; | ||||
|       const double scale = 0.5 * one_over_r * mag; | ||||
|       return sgn_w * scale * Vector3(w1, w2, w3); | ||||
|       }; | ||||
| 
 | ||||
|     if (R33 > R22 && R33 > R11) { // R33 is the largest diagonal, a=3, b=1, c=2
 | ||||
|       const double Q1 = 2.0 + 2.0 * R33; | ||||
|       omega = omegaFromQ(R21 - R12, sqrt(Q1), R31 + R13, R23 + R32, Q1); | ||||
|     } else if (R22 > R11) { // R22 is the largest diagonal, a=2, b=3, c=1
 | ||||
|       omega = sgn_w * scale * Vector3(Q2, Q3, Q1); | ||||
|     } else if (R22 > R11) { | ||||
|       // R22 is the largest diagonal, a=2, b=3, c=1
 | ||||
|       const double W = R13 - R31; | ||||
|       const double Q1 = 2.0 + 2.0 * R22; | ||||
|       omega = omegaFromQ(R13 - R31, sqrt(Q1), R12 + R21, Q1, R23 + R32); | ||||
|     } else { // R11 is the largest diagonal, a=1, b=2, c=3
 | ||||
|       const double Q2 = R23 + R32; | ||||
|       const double Q3 = R12 + R21; | ||||
|       const double r = sqrt(Q1); | ||||
|       const double one_over_r = 1 / r; | ||||
|       const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W); | ||||
|       const double sgn_w = W < 0 ? -1.0 : 1.0; | ||||
|       const double mag = M_PI - (2 * sgn_w * W) / norm; | ||||
|       const double scale = 0.5 * one_over_r * mag; | ||||
|       omega = sgn_w * scale * Vector3(Q3, Q1, Q2); | ||||
|     } else { | ||||
|       // R11 is the largest diagonal, a=1, b=2, c=3
 | ||||
|       const double W = R32 - R23; | ||||
|       const double Q1 = 2.0 + 2.0 * R11; | ||||
|       omega = omegaFromQ(R32 - R23, sqrt(Q1), Q1, R12 + R21, R31 + R13); | ||||
|       const double Q2 = R12 + R21; | ||||
|       const double Q3 = R31 + R13; | ||||
|       const double r = sqrt(Q1); | ||||
|       const double one_over_r = 1 / r; | ||||
|       const double norm = sqrt(Q1*Q1 + Q2*Q2 + Q3*Q3 + W*W); | ||||
|       const double sgn_w = W < 0 ? -1.0 : 1.0; | ||||
|       const double mag = M_PI - (2 * sgn_w * W) / norm; | ||||
|       const double scale = 0.5 * one_over_r * mag; | ||||
|       omega = sgn_w * scale * Vector3(Q1, Q2, Q3); | ||||
|     } | ||||
|   } else { | ||||
|     double magnitude; | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue