From 4cdfd3645fd6355237de64dc5fe6c411740d40d3 Mon Sep 17 00:00:00 2001
From: artivis <deray.jeremie@gmail.com>
Date: Mon, 21 May 2018 21:14:19 +0200
Subject: [PATCH] ConstraintOdom3D fix ptr to tmp var

---
 src/constraint_odom_3D.h | 54 +++++++++++++++++++++++++---------------
 1 file changed, 34 insertions(+), 20 deletions(-)

diff --git a/src/constraint_odom_3D.h b/src/constraint_odom_3D.h
index a3d4b7af0..26811e470 100644
--- a/src/constraint_odom_3D.h
+++ b/src/constraint_odom_3D.h
@@ -111,18 +111,32 @@ inline bool ConstraintOdom3D::expectation(const T* const _p_current, const T* co
     Eigen::Map<Eigen::Matrix<T,3,1> > expectation_dp(_expectation_dp);
     Eigen::Map<Eigen::Quaternion<T> > expectation_dq(_expectation_dq);
 
-
-//     std::cout << "p_current: " << p_current(0) << std::endl << p_current(1) << std::endl << p_current(2) << std::endl;
-//     std::cout << "q_current: " << q_current.x() << std::endl << q_current.y() << std::endl << q_current.z() << std::endl << q_current.w() << std::endl;
-//     std::cout << "p_past: " << p_past(0) << std::endl << p_past(1) << std::endl << p_past(2) << std::endl;
-//     std::cout << "q_past: " << q_past.x() << std::endl << q_past.y() << std::endl << q_past.z() << std::endl << q_past.w() << std::endl;
+//     std::cout << "p_current: " << p_current(0) << " "
+//                                << p_current(1) << " "
+//                                << p_current(2) << "\n";
+//     std::cout << "q_current: " << q_current.coeffs()(0) << " "
+//                                << q_current.coeffs()(1) << " "
+//                                << q_current.coeffs()(2) << " "
+//                                << q_current.coeffs()(3) << "\n";
+//     std::cout << "p_past: " << p_past(0) << " "
+//                             << p_past(1) << " "
+//                             << p_past(2) << "\n";
+//     std::cout << "q_past: " << q_past.coeffs()(0) << " "
+//                             << q_past.coeffs()(1) << " "
+//                             << q_past.coeffs()(2) << " "
+//                             << q_past.coeffs()(3) << "\n";
 
     // estimate motion increment, dp, dq;
     expectation_dp = q_past.conjugate() * (p_current - p_past);
-    expectation_dq =  q_past.conjugate() * q_current;
+    expectation_dq = q_past.conjugate() * q_current;
 
-//    std::cout << "exp_p: " << expectation_dp(0) << std::endl << expectation_dp(1) << std::endl << expectation_dp(2) << std::endl;
-//    std::cout << "exp_q: " << expectation(3) << std::endl << expectation(4) << std::endl << expectation(5) << std::endl << expectation(6) << std::endl;
+//    std::cout << "exp_p: " << expectation_dp(0) << " "
+//                           << expectation_dp(1) << " "
+//                           << expectation_dp(2) << "\n";
+//    std::cout << "exp_q: " << expectation_dq.coeffs()(0) << " "
+//                           << expectation_dq.coeffs()(1) << " "
+//                           << expectation_dq.coeffs()(2) << " "
+//                           << expectation_dq.coeffs()(3) << "\n";
 
     return true;
 }
@@ -132,18 +146,18 @@ inline Eigen::VectorXs ConstraintOdom3D::expectation() const
     Eigen::VectorXs exp(7);
     FrameBasePtr frm_current = getFeaturePtr()->getFramePtr();
     FrameBasePtr frm_past = getFrameOtherPtr();
-    const Scalar * const frame_current_pos  = frm_current->getPPtr()->getState().data();
-    const Scalar * const frame_current_ori  = frm_current->getOPtr()->getState().data();
-    const Scalar * const frame_past_pos     = frm_past->getPPtr()->getState().data();
-    const Scalar * const frame_past_ori     = frm_past->getOPtr()->getState().data();
-
-//    std::cout << "frame_current_pos: " << frm_current->getPPtr()->getVector().transpose() << std::endl;
-//    std::cout << "frame_past_pos: " << frm_past->getPPtr()->getVector().transpose() << std::endl;
-
-    expectation(frame_current_pos,
-                frame_current_ori,
-                frame_past_pos,
-                frame_past_ori,
+    const Eigen::VectorXs frame_current_pos  = frm_current->getPPtr()->getState();
+    const Eigen::VectorXs frame_current_ori  = frm_current->getOPtr()->getState();
+    const Eigen::VectorXs frame_past_pos     = frm_past->getPPtr()->getState();
+    const Eigen::VectorXs frame_past_ori     = frm_past->getOPtr()->getState();
+
+//    std::cout << "frame_current_pos: " << frm_current->getPPtr()->getState().transpose() << std::endl;
+//    std::cout << "frame_past_pos: " << frm_past->getPPtr()->getState().transpose() << std::endl;
+
+    expectation(frame_current_pos.data(),
+                frame_current_ori.data(),
+                frame_past_pos.data(),
+                frame_past_ori.data(),
                 exp.data(),
                 exp.data()+3);
     return exp;
-- 
GitLab