Skip to content
Snippets Groups Projects
Commit 82589bfc authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

optimized factor and new test

parent 967da9b6
No related branches found
No related tags found
2 merge requests!258Apriltag: some improvements/changes,!233WIP: Apriltag
Pipeline #2722 passed
...@@ -101,7 +101,36 @@ FactorAutodiffApriltag::~FactorAutodiffApriltag() ...@@ -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 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]), 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_keyframe (_p_keyframe[0], _p_keyframe[1], _p_keyframe[2]),
p_landmark (_p_landmark[0], _p_landmark[1], _p_landmark[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 ...@@ -113,7 +142,7 @@ template<typename T> bool FactorAutodiffApriltag::operator ()( const T* const _p
Eigen::Translation3ds p_measured(getMeasurement().head(3)); Eigen::Translation3ds p_measured(getMeasurement().head(3));
Eigen::Quaternions q_measured(getMeasurement().data() + 3 ); Eigen::Quaternions q_measured(getMeasurement().data() + 3 );
// landmark wrt camera, measure // 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 // Create transformation matrices to compose
// robot wrt world // robot wrt world
...@@ -152,7 +181,7 @@ template<typename T> bool FactorAutodiffApriltag::operator ()( const T* const _p ...@@ -152,7 +181,7 @@ template<typename T> bool FactorAutodiffApriltag::operator ()( const T* const _p
Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals); Eigen::Map<Eigen::Matrix<T, 6, 1>> res(_residuals);
res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * err; res = getFeature()->getMeasurementSquareRootInformationUpper().cast<T>() * err;
*/
return true; return true;
} }
......
...@@ -403,6 +403,90 @@ TEST_F(FactorAutodiffApriltag_class, solve_L1_O_perturbated) ...@@ -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] //[Class methods]
int main(int argc, char **argv) int main(int argc, char **argv)
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment