diff --git a/minus/minus.h b/minus/minus.h index 04b8071..cf0914c 100644 --- a/minus/minus.h +++ b/minus/minus.h @@ -247,15 +247,16 @@ struct minus_io_14a : public minus_io_common { typedef minus_util u; // camera 0 (2nd camera relative to 1st) u::quat2rotm(rs, (F *) cameras[0]); - cameras[0][3][0] = rs[8]; - cameras[0][3][1] = rs[9]; - cameras[0][3][2] = rs[10]; + F n = sqrt(rs[8]*rs[8] + rs[9]*rs[9] + rs[10]*rs[10]) + sqrt(rs[11]*rs[11] + rs[12]*rs[12] + rs[13]*rs[13]); + cameras[0][3][0] = rs[8]/n; + cameras[0][3][1] = rs[9]/n; + cameras[0][3][2] = rs[10]/n; // camera 1 (3rd camera relative to 1st) u::quat2rotm(rs+4, (F *) cameras[1]); - cameras[1][3][0] = rs[11]; - cameras[1][3][1] = rs[12]; - cameras[1][3][2] = rs[13]; + cameras[1][3][0] = rs[11]/n; + cameras[1][3][1] = rs[12]/n; + cameras[1][3][2] = rs[13]/n; // quat12 rs(0:3), quat12 rs(4:7) // T12 = solutions(9:11); diff --git a/minus/minus.hxx b/minus/minus.hxx index 9cbbb8f..c32b0f9 100644 --- a/minus/minus.hxx +++ b/minus/minus.hxx @@ -60,8 +60,6 @@ lsolve( } m_p = m_rowsTranspositions; - - // Step 1 x = m_p * b; // TODO: use block indexing and std::vector-std::vector multiplication