diff --git a/include/base/factor/factor_autodiff_apriltag.h b/include/base/factor/factor_autodiff_apriltag.h
index 3402cc497ec9c71f293dff2038be6a7c1d985b50..42d5068b1b1d155c44f617b4a7e650301a64e6e3 100644
--- a/include/base/factor/factor_autodiff_apriltag.h
+++ b/include/base/factor/factor_autodiff_apriltag.h
@@ -101,7 +101,36 @@ FactorAutodiffApriltag::~FactorAutodiffApriltag()
template<typename T> bool FactorAutodiffApriltag::operator ()( const T* const _p_camera, const T* const _o_camera, const T* const _p_keyframe, const T* const _o_keyframe, const T* const _p_landmark, const T* const _o_landmark, T* _residuals) const
{
- //states
+ // Maps
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_r_c(_p_camera);
+ Eigen::Map<const Eigen::Quaternion<T>> q_r_c(_o_camera);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_r(_p_keyframe);
+ Eigen::Map<const Eigen::Quaternion<T>> q_w_r(_o_keyframe);
+ Eigen::Map<const Eigen::Matrix<T,3,1>> p_w_l(_p_landmark);
+ Eigen::Map<const Eigen::Quaternion<T>> q_w_l(_o_landmark);
+ Eigen::Map<Eigen::Matrix<T,6,1>> residuals(_residuals);
+
+ // Expected measurement
+ Eigen::Quaternion<T> q_c_w = (q_w_r * q_r_c).conjugate();
+ Eigen::Quaternion<T> q_c_l = q_c_w * q_w_l;
+ Eigen::Matrix<T,3,1> p_c_l = q_c_w * (-(p_w_r + q_w_r * p_r_c) + p_w_l);
+
+ // Measurement
+ Eigen::Vector3s p_c_l_meas(getMeasurement().head<3>());
+ Eigen::Quaternions q_c_l_meas(getMeasurement().data() + 3 );
+ Eigen::Quaternion<T> q_l_c_meas = q_c_l_meas.conjugate().cast<T>();
+ //Eigen::Matrix<T,3,1> p_l_c_meas = -q_l_c_meas * p_c_l_meas.cast<T>();
+
+ // Error
+ Eigen::Matrix<T, 6, 1> err;
+ err.head(3) = q_l_c_meas * (p_c_l_meas.cast<T>() - p_c_l);
+ //err.tail(3) = wolf::log_q(q_l_c_meas * q_c_l);
+ err.tail(3) = T(2)*(q_l_c_meas * q_c_l).vec();
+
+ // Residual
+ residuals = getMeasurementSquareRootInformationUpper().cast<T>() * err;
+
+ /*//states
Eigen::Translation<T,3> p_camera (_p_camera[0] , _p_camera[1] , _p_camera[2]),
p_keyframe (_p_keyframe[0], _p_keyframe[1], _p_keyframe[2]),
p_landmark (_p_landmark[0], _p_landmark[1], _p_landmark[2]);
@@ -113,7 +142,7 @@ template<typename T> bool FactorAutodiffApriltag::operator ()( const T* const _p
Eigen::Translation3ds p_measured(getMeasurement().head(3));
Eigen::Quaternions q_measured(getMeasurement().data() + 3 );
// landmark wrt camera, measure
- Eigen::Transform<T, 3, Eigen::Affine> c_M_l_meas = p_measured.cast<T>() * q_measured.cast<T>();
+ Eigen::Transform<T, 3, Eigen::Affine> c_M_l_meas = p_c_l_meas.cast<T>() * q_measured.cast<T>();
// Create transformation matrices to compose
// robot wrt world
@@ -152,7 +181,7 @@ template<typename T> bool FactorAutodiffApriltag::operator ()( const T* const _p
Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals);
res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * err;
-
+ */
return true;
}
diff --git a/test/gtest_factor_autodiff_apriltag.cpp b/test/gtest_factor_autodiff_apriltag.cpp
index f88c461d526664424f0b18fad74451fde28da812..c815e26eb702748f38d3d4f916413e6fb76e0488 100644
--- a/test/gtest_factor_autodiff_apriltag.cpp
+++ b/test/gtest_factor_autodiff_apriltag.cpp
@@ -403,6 +403,90 @@ TEST_F(FactorAutodiffApriltag_class, solve_L1_O_perturbated)
}
+TEST_F(FactorAutodiffApriltag_class, solve_L1_PO_perturbated)
+{
+ FactorAutodiffApriltagPtr constraint = std::make_shared<FactorAutodiffApriltag>(
+ S,
+ F1,
+ lmk1,
+ f1,
+ false,
+ CTR_ACTIVE
+ );
+
+ FactorAutodiffApriltagPtr ctr0 = std::static_pointer_cast<FactorAutodiffApriltag>(f1->addFactor(constraint));
+ lmk1->addConstrainedBy(constraint);
+ F1->addConstrainedBy(constraint);
+ f1->addConstrainedBy(constraint);
+
+ // Change setup
+ Vector3s p_w_r, p_r_c, p_c_l, p_w_l;
+ Quaternions q_w_r, q_r_c, q_c_l, q_w_l;
+ p_w_r << 1, 2, 3;
+ p_r_c << 4, 5, 6;
+ p_c_l << 7, 8, 9;
+ q_w_r.coeffs() << 1, 2, 3, 4; q_w_r.normalize();
+ q_r_c.coeffs() << 4, 5, 6, 7; q_r_c.normalize();
+ q_c_l.coeffs() << 7, 8, 9, 0; q_c_l.normalize();
+
+ q_w_l = q_w_r * q_r_c * q_c_l;
+ p_w_l = p_w_r + q_w_r * (p_r_c + q_r_c * p_c_l);
+
+ // Change feature
+ Vector7s meas;
+ meas << p_c_l, q_c_l.coeffs();
+ f1->setMeasurement(meas);
+
+ // Change Landmark
+ lmk1->getP()->setState(p_w_l);
+ lmk1->getO()->setState(q_w_l.coeffs());
+ ASSERT_TRUE(std::find(problem->getStateBlockPtrList().begin(), problem->getStateBlockPtrList().end(), lmk1->getP()) != problem->getStateBlockPtrList().end());
+ ASSERT_TRUE(std::find(problem->getStateBlockPtrList().begin(), problem->getStateBlockPtrList().end(), lmk1->getO()) != problem->getStateBlockPtrList().end());
+ ASSERT_TRUE(lmk1->getP()->stateUpdated());
+ ASSERT_TRUE(lmk1->getO()->stateUpdated());
+
+ // Change Frame
+ F1->getP()->setState(p_w_r);
+ F1->getO()->setState(q_w_r.coeffs());
+ ASSERT_TRUE(std::find(problem->getStateBlockPtrList().begin(), problem->getStateBlockPtrList().end(), F1->getP()) != problem->getStateBlockPtrList().end());
+ ASSERT_TRUE(std::find(problem->getStateBlockPtrList().begin(), problem->getStateBlockPtrList().end(), F1->getO()) != problem->getStateBlockPtrList().end());
+ ASSERT_TRUE(F1->getP()->stateUpdated());
+ ASSERT_TRUE(F1->getO()->stateUpdated());
+
+ // Change sensor extrinsics
+ S->getP()->setState(p_r_c);
+ S->getO()->setState(q_r_c.coeffs());
+ ASSERT_TRUE(std::find(problem->getStateBlockPtrList().begin(), problem->getStateBlockPtrList().end(), S->getP()) != problem->getStateBlockPtrList().end());
+ ASSERT_TRUE(std::find(problem->getStateBlockPtrList().begin(), problem->getStateBlockPtrList().end(), S->getO()) != problem->getStateBlockPtrList().end());
+ ASSERT_TRUE(S->getP()->stateUpdated());
+ ASSERT_TRUE(S->getO()->stateUpdated());
+
+ Vector7s t_w_r, t_w_l;
+ t_w_r << p_w_r, q_w_r.coeffs();
+ t_w_l << p_w_l, q_w_l.coeffs();
+ ASSERT_MATRIX_APPROX(F1->getState(), t_w_r, 1e-6);
+ ASSERT_MATRIX_APPROX(lmk1->getState(), t_w_l, 1e-6);
+
+ // unfix LMK, perturbate state
+ lmk1->unfix();
+ Vector3s e0_pos = p_w_l + Vector3s::Random() * 0.25;
+ Quaternions e0_quat = q_w_l * exp_q(Vector3s::Random() * 0.1);
+ lmk1->getP()->setState(e0_pos);
+ lmk1->getO()->setState(e0_quat.coeffs());
+ F1->fix();
+ S->fixExtrinsics();
+
+// solve
+ std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET); // 0: nothing, 1: BriefReport, 2: FullReport
+ //WOLF_DEBUG("Landmark state after solve: ");
+ //WOLF_DEBUG(lmk1->getState().transpose());
+ t_w_r << p_w_r, q_w_r.coeffs();
+ t_w_l << p_w_l, q_w_l.coeffs();
+ ASSERT_MATRIX_APPROX(F1->getState(), t_w_r, 1e-6);
+ ASSERT_MATRIX_APPROX(lmk1->getState(), t_w_l, 1e-6);
+
+}
+
//[Class methods]
int main(int argc, char **argv)