summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
-rw-r--r--cv/translation-calibrator.cpp22
1 files changed, 10 insertions, 12 deletions
diff --git a/cv/translation-calibrator.cpp b/cv/translation-calibrator.cpp
index 75526815..f76fc32e 100644
--- a/cv/translation-calibrator.cpp
+++ b/cv/translation-calibrator.cpp
@@ -58,21 +58,19 @@ cv::Vec3f TranslationCalibrator::get_estimate()
bool TranslationCalibrator::check_bucket(const cv::Matx33d& R_CM_k)
{
- const int idx = progn(
- using namespace euler;
- static constexpr double r2d = 180/M_PI;
+ using namespace euler;
+ static constexpr double r2d = 180/M_PI;
- rmat r;
- for (unsigned j = 0; j < 3; j++)
- for (unsigned i = 0; i < 3; i++)
- r(j, i) = R_CM_k(j, i);
+ rmat r;
+ for (unsigned j = 0; j < 3; j++)
+ for (unsigned i = 0; i < 3; i++)
+ r(j, i) = R_CM_k(j, i);
- const euler_t ypr = rmat_to_euler(r) * r2d;
+ const euler_t ypr = rmat_to_euler(r) * r2d;
- const int yaw = iround(ypr(yaw_rdof) + 180)/spacing_in_degrees;
- const int pitch = iround(ypr(pitch_rdof) + 180)/spacing_in_degrees;
- return pitch * 360/spacing_in_degrees + yaw;
- );
+ const int yaw = iround(ypr(yaw_rdof) + 180)/spacing_in_degrees;
+ const int pitch = iround(ypr(pitch_rdof) + 180)/spacing_in_degrees;
+ const int idx = pitch * 360/spacing_in_degrees + yaw;
if (idx >= 0 && idx < bin_count)
{