Commit 3092ec4a authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Merge branch '21-follow-core-465-migrate-from-stateblock-to-statederived' into...

Merge branch '21-follow-core-465-migrate-from-stateblock-to-statederived' into 12-add-aerial--follow-core-465-migrate-from-stateblock-to-statederived
parents 1e0ae19e 4f10166a
......@@ -23,6 +23,8 @@
#include "bodydynamics/capture/capture_inertial_kinematics.h"
#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
#include "core/state_block/state_quaternion.h"
#include "core/state_block/state_block_derived.h"
namespace wolf {
......@@ -37,7 +39,7 @@ CaptureInertialKinematics::CaptureInertialKinematics(const TimeStamp& _init_ts,
_sensor_ptr,
nullptr,
nullptr,
std::make_shared<StateBlock>(_bias_pbc, false)),
std::make_shared<StateParams3>(_bias_pbc, false)),
data_(_data),
B_I_q_(_B_I_q),
B_Lc_m_(_B_Lc_m)
......@@ -56,7 +58,7 @@ CaptureInertialKinematics::CaptureInertialKinematics(const TimeStamp& _init_ts,
_sensor_ptr,
nullptr,
nullptr,
std::make_shared<StateBlock>(3, false)),
std::make_shared<StateParams3>(Vector3d::Zero(), false)),
data_(_data),
B_I_q_(_B_I_q),
B_Lc_m_(_B_Lc_m)
......
......@@ -21,7 +21,7 @@
//--------LICENSE_END--------
#include "bodydynamics/sensor/sensor_inertial_kinematics.h"
#include "core/state_block/state_block.h"
#include "core/state_block/state_block_derived.h"
#include "core/state_block/state_quaternion.h"
namespace wolf {
......@@ -30,7 +30,7 @@ SensorInertialKinematics::SensorInertialKinematics(const Eigen::VectorXd& _extri
SensorBase("SensorInertialKinematics",
nullptr, // no position
nullptr, // no orientation
std::make_shared<StateBlock>(Eigen::Vector3d(0,0,0), false), // bias; false = unfixed
std::make_shared<StateParams3>(Eigen::Vector3d(0,0,0), false), // bias; false = unfixed
(Eigen::Vector6d() << _params->std_pbc,_params->std_pbc,_params->std_pbc,
_params->std_vbc,_params->std_vbc,_params->std_vbc).finished(),
false, false, true),
......
......@@ -58,6 +58,7 @@ FactorInertialKinematics_2KF_1v_bfix,ZeroMvt:
#include <core/capture/capture_base.h>
#include <core/feature/feature_base.h>
#include <core/factor/factor_block_absolute.h>
#include <core/state_block/state_block_derived.h>
// Imu
// #include "imu/internal/config.h"
......@@ -169,18 +170,26 @@ void perturbateAllIfUnFixed(const FrameBasePtr& KF)
}
}
FrameBasePtr createKFWithCDLI(const ProblemPtr& problem, const TimeStamp& t, VectorComposite x_origin,
Vector3d c, Vector3d cd, Vector3d Lc, Vector6d bias_imu)
FrameBasePtr createKFWithCDLI(const ProblemPtr& problem,
const TimeStamp& t,
VectorComposite x_origin,
Vector3d c,
Vector3d cd,
Vector3d Lc,
Vector6d bias_imu)
{
FrameBasePtr KF = FrameBase::emplace<FrameBase>(problem->getTrajectory(), t, "POV", x_origin);
StateBlockPtr sbc = make_shared<StateBlock>(c); KF->addStateBlock('C', sbc, problem);
StateBlockPtr sbd = make_shared<StateBlock>(cd); KF->addStateBlock('D', sbd, problem);
StateBlockPtr sbL = make_shared<StateBlock>(Lc); KF->addStateBlock('L', sbL, problem);
StateBlockPtr sbbimu = make_shared<StateBlock>(bias_imu); KF->addStateBlock('I', sbbimu, problem); // Simulating IMU
FrameBasePtr KF = FrameBase::emplace<FrameBase>(problem->getTrajectory(), t, "POV", x_origin);
StateBlockPtr sbc = make_shared<StatePoint3d>(c);
KF->addStateBlock('C', sbc, problem);
StateBlockPtr sbd = make_shared<StateVector3d>(cd);
KF->addStateBlock('D', sbd, problem);
StateBlockPtr sbL = make_shared<StateVector3d>(Lc);
KF->addStateBlock('L', sbL, problem);
StateBlockPtr sbbimu = make_shared<StateParams6>(bias_imu);
KF->addStateBlock('I', sbbimu, problem); // Simulating IMU
return KF;
}
class FactorInertialKinematics_2KF : public testing::Test
{
public:
......@@ -247,10 +256,10 @@ class FactorInertialKinematics_2KF : public testing::Test
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
bias_p_ = zero3;
bias_imu_ = zero6;
StateBlockPtr sbcA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('C', sbcA, problem_);
StateBlockPtr sbdA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('D', sbdA, problem_);
StateBlockPtr sbLA = make_shared<StateBlock>(zero3); KFA_->addStateBlock('L', sbLA, problem_);
StateBlockPtr sbbimuA = make_shared<StateBlock>(bias_imu_); KFA_->addStateBlock('I', sbbimuA, problem_);
StateBlockPtr sbcA = make_shared<StatePoint3d>(zero3); KFA_->addStateBlock('C', sbcA, problem_);
StateBlockPtr sbdA = make_shared<StateVector3d>(zero3); KFA_->addStateBlock('D', sbdA, problem_);
StateBlockPtr sbLA = make_shared<StateVector3d>(zero3); KFA_->addStateBlock('L', sbLA, problem_);
StateBlockPtr sbbimuA = make_shared<StateParams6>(bias_imu_); KFA_->addStateBlock('I', sbbimuA, problem_);
// Fix the one we cannot estimate
// KFA_->getP()->fix();
......
......@@ -58,6 +58,7 @@ FactorInertialKinematics_1KF_1v_bfix,ZeroMvt:
#include <core/capture/capture_base.h>
#include <core/feature/feature_base.h>
#include <core/factor/factor_block_absolute.h>
#include <core/state_block/state_block_derived.h>
// Imu
//#include "imu/internal/config.h"
......@@ -72,6 +73,7 @@ FactorInertialKinematics_1KF_1v_bfix,ZeroMvt:
#include "bodydynamics/capture/capture_inertial_kinematics.h"
#include "bodydynamics/feature/feature_inertial_kinematics.h"
#include "bodydynamics/factor/factor_inertial_kinematics.h"
#include <core/state_block/state_block_derived.h>
#include <Eigen/Eigenvalues>
......@@ -126,11 +128,11 @@ class FactorInertialKinematics_1KF : public testing::Test
// ADD THE STATEBLOCKS AND SET THEIR DEFAULT VALUES
bias_p_ = ZERO3;
bias_imu_ = ZERO6;
StateBlockPtr sbc = make_shared<StateBlock>(ZERO3);
StateBlockPtr sbd = make_shared<StateBlock>(ZERO3);
StateBlockPtr sbL = make_shared<StateBlock>(ZERO3);
StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
sbbimu_ = make_shared<StateBlock>(bias_imu_);
StateBlockPtr sbc = make_shared<StatePoint3d>(ZERO3);
StateBlockPtr sbL = make_shared<StateVector3d>(ZERO3);
StateBlockPtr sbd = make_shared<StateVector3d>(ZERO3);
StateBlockPtr sbb = make_shared<StateParams3>(bias_p_);
sbbimu_ = make_shared<StateParams6>(bias_imu_);
KF0_->addStateBlock('C', sbc, problem_);
KF0_->addStateBlock('D', sbd, problem_);
......@@ -328,7 +330,7 @@ class FactorInertialKinematics_1KF_1v_bfix : public FactorInertialKinematics_1KF
// Vector6d ZERO6; ZERO6 << ZERO3, ZERO3;
// Vector3d bias_p_ = ZERO3;
// Vector6d bias_imu_ = ZERO6;
// StateBlockPtr sbc = make_shared<StateBlock>(ZERO3);
// StateBlockPtr sbc = make_shared<StatePoint3d>(ZERO3);
// StateBlockPtr sbd = make_shared<StateBlock>(ZERO3);
// StateBlockPtr sbL = make_shared<StateBlock>(ZERO3);
// StateBlockPtr sbb = make_shared<StateBlock>(bias_p_);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment