diff --git a/demos/demo_analytic_autodiff_factor.cpp b/demos/demo_analytic_autodiff_factor.cpp
index f514180685a326f5945b8c51cf07dcd698748186..7d0fdbec689cba9f912e8e26c3c823bb6c29cb23 100644
--- a/demos/demo_analytic_autodiff_factor.cpp
+++ b/demos/demo_analytic_autodiff_factor.cpp
@@ -59,7 +59,7 @@ int main(int argc, char** argv)
// Wolf problem
ProblemPtr wolf_problem_autodiff = new Problem(FRM_PO_2D);
ProblemPtr wolf_problem_analytic = new Problem(FRM_PO_2D);
- SensorBasePtr sensor = new SensorBase("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
+ SensorBasePtr sensor = new SensorBase("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
// Ceres wrapper
ceres::Solver::Options ceres_options;
@@ -102,7 +102,7 @@ int main(int argc, char** argv)
while (buffer.at(i) == ' ') i++;
// vertex pose
- Eigen::Vector3s vertex_pose;
+ Eigen::Vector3d vertex_pose;
// x
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
@@ -164,7 +164,7 @@ int main(int argc, char** argv)
while (buffer.at(i) == ' ') i++;
// edge vector
- Eigen::Vector3s edge_vector;
+ Eigen::Vector3d edge_vector;
// x
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
@@ -188,7 +188,7 @@ int main(int argc, char** argv)
while (buffer.at(i) == ' ') i++;
// edge covariance
- Eigen::Matrix3s edge_information;
+ Eigen::Matrix3d edge_information;
// xx
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
@@ -274,8 +274,8 @@ int main(int argc, char** argv)
// PRIOR
FrameBasePtr first_frame_autodiff = wolf_problem_autodiff->getTrajectory()->getFrameList().front();
FrameBasePtr first_frame_analytic = wolf_problem_analytic->getTrajectory()->getFrameList().front();
- CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3s::Identity() * 0.01);
- CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3s::Identity() * 0.01);
+ CaptureFix* initial_covariance_autodiff = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_autodiff->getState(), Eigen::Matrix3d::Identity() * 0.01);
+ CaptureFix* initial_covariance_analytic = new CaptureFix(TimeStamp(0), new SensorBase("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_analytic->getState(), Eigen::Matrix3d::Identity() * 0.01);
first_frame_autodiff->addCapture(initial_covariance_autodiff);
first_frame_analytic->addCapture(initial_covariance_analytic);
initial_covariance_autodiff->emplaceFeatureAndFactor();
diff --git a/demos/demo_map_yaml.cpp b/demos/demo_map_yaml.cpp
index e509c94e2116b2f7bc08e70d866eafb5ed5a9bfb..c56eba791c994cacc04577154f5720f0b81ce486 100644
--- a/demos/demo_map_yaml.cpp
+++ b/demos/demo_map_yaml.cpp
@@ -54,7 +54,7 @@ int main()
using namespace Eigen;
std::cout << "\nTesting Lmk creator and node saving..." << std::endl;
- Vector4s v;
+ Vector4d v;
v << 1, 2, 3, 4;
cv::Mat d = (cv::Mat_<int>(8,1) << 1, 2, 3, 4, 5, 6, 7, 8);
LandmarkAHP lmk_1(v, nullptr, nullptr, d);
@@ -62,8 +62,8 @@ int main()
std::cout << "Des 1 = " << lmk_1.getCvDescriptor().t() << std::endl;
YAML::Node n = lmk_1.saveToYaml();
- std::cout << "Pos n = " << n["position"].as<VectorXs>().transpose() << std::endl;
- std::cout << "Des n = " << n["descriptor"].as<VectorXs>().transpose() << std::endl;
+ std::cout << "Pos n = " << n["position"].as<VectorXd>().transpose() << std::endl;
+ std::cout << "Des n = " << n["descriptor"].as<VectorXd>().transpose() << std::endl;
LandmarkAHP lmk_2 = *(std::static_pointer_cast<LandmarkAHP>(LandmarkAHP::create(n)));
std::cout << "Pos 2 = " << lmk_2.getP()->getState().transpose() << std::endl;
diff --git a/demos/demo_wolf_imported_graph.cpp b/demos/demo_wolf_imported_graph.cpp
index d2f570970fca381501b954bdaad04621a3fe6d8d..40646846123e3fcd0bf09b27e20d0c03f27d1165 100644
--- a/demos/demo_wolf_imported_graph.cpp
+++ b/demos/demo_wolf_imported_graph.cpp
@@ -52,7 +52,7 @@ int main(int argc, char** argv)
std::ifstream offLineFile_;
clock_t t1;
ceres::Solver::Summary summary_full, summary_prun;
- Eigen::MatrixXs Sigma_ii(3,3), Sigma_ij(3,3), Sigma_jj(3,3), Sigma_z(3,3), Ji(3,3), Jj(3,3);
+ Eigen::MatrixXd Sigma_ii(3,3), Sigma_ij(3,3), Sigma_jj(3,3), Sigma_z(3,3), Ji(3,3), Jj(3,3);
double xi, yi, thi, si, ci, xj, yj;
// loading variables
@@ -63,7 +63,7 @@ int main(int argc, char** argv)
// Wolf problem
ProblemPtr wolf_problem_full = new Problem(FRM_PO_2D);
ProblemPtr wolf_problem_prun = new Problem(FRM_PO_2D);
- SensorBasePtr sensor = new SensorBase("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
+ SensorBasePtr sensor = new SensorBase("ODOM 2D", std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)), std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
Eigen::SparseMatrix<double> Lambda(0,0);
@@ -108,7 +108,7 @@ int main(int argc, char** argv)
while (buffer.at(i) == ' ') i++;
// vertex pose
- Eigen::Vector3s vertex_pose;
+ Eigen::Vector3d vertex_pose;
// x
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
@@ -173,7 +173,7 @@ int main(int argc, char** argv)
while (buffer.at(i) == ' ') i++;
// edge vector
- Eigen::Vector3s edge_vector;
+ Eigen::Vector3d edge_vector;
// x
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
@@ -197,7 +197,7 @@ int main(int argc, char** argv)
while (buffer.at(i) == ' ') i++;
// edge covariance
- Eigen::Matrix3s edge_information;
+ Eigen::Matrix3d edge_information;
// xx
while (buffer.at(i) != ' ')
bNum.push_back(buffer.at(i++));
@@ -275,7 +275,7 @@ int main(int argc, char** argv)
double ci = cos(thi);
double xj = *(frame_new_ptr_prun->getP()->get());
double yj = *(frame_new_ptr_prun->getP()->get()+1);
- Eigen::MatrixXs Ji(3,3), Jj(3,3);
+ Eigen::MatrixXd Ji(3,3), Jj(3,3);
Ji << -ci,-si,-(xj-xi)*si+(yj-yi)*ci,
si,-ci,-(xj-xi)*ci-(yj-yi)*si,
0, 0, -1;
@@ -303,15 +303,15 @@ int main(int argc, char** argv)
// PRIOR
FrameBasePtr first_frame_full = wolf_problem_full->getTrajectory()->getFrameList().front();
FrameBasePtr first_frame_prun = wolf_problem_prun->getTrajectory()->getFrameList().front();
- CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3s::Identity() * 0.01);
- CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3s::Identity() * 0.01);
+ CaptureFix* initial_covariance_full = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_full->getState(), Eigen::Matrix3d::Identity() * 0.01);
+ CaptureFix* initial_covariance_prun = new CaptureFix(TimeStamp(0), new SensorBase("ABSLOUTE POSE", nullptr, nullptr, nullptr, 0), first_frame_prun->getState(), Eigen::Matrix3d::Identity() * 0.01);
first_frame_full->addCapture(initial_covariance_full);
first_frame_prun->addCapture(initial_covariance_prun);
initial_covariance_full->process();
initial_covariance_prun->process();
//std::cout << "initial covariance: factor " << initial_covariance_prun->getFeatureList().front()->getFactorFromList().front()->id() << std::endl << initial_covariance_prun->getFeatureList().front()->getMeasurementCovariance() << std::endl;
Eigen::SparseMatrix<double> DeltaLambda(Lambda.rows(), Lambda.cols());
- insertSparseBlock((Eigen::Matrix3s::Identity() * 100).sparseView(), DeltaLambda, 0, 0);
+ insertSparseBlock((Eigen::Matrix3d::Identity() * 100).sparseView(), DeltaLambda, 0, 0);
Lambda = Lambda + DeltaLambda;
// COMPUTE COVARIANCES
@@ -320,7 +320,7 @@ int main(int argc, char** argv)
// Manual covariance computation
t1 = clock();
Eigen::SimplicialLLT<Eigen::SparseMatrix<double>> chol(Lambda); // performs a Cholesky factorization of A
- Eigen::MatrixXs Sigma = chol.solve(Eigen::MatrixXs::Identity(Lambda.rows(), Lambda.cols()));
+ Eigen::MatrixXd Sigma = chol.solve(Eigen::MatrixXd::Identity(Lambda.rows(), Lambda.cols()));
double t_sigma_manual = ((double) clock() - t1) / CLOCKS_PER_SEC;
//std::cout << "Lambda" << std::endl << Lambda << std::endl;
//std::cout << "Sigma" << std::endl << Sigma << std::endl;
diff --git a/demos/solver/test_iQR_wolf2.cpp b/demos/solver/test_iQR_wolf2.cpp
index 74094389284e4cc47e647935c0ec481f5f6c96aa..8084a74093ba94602ce19dd413deb12c24cfa047 100644
--- a/demos/solver/test_iQR_wolf2.cpp
+++ b/demos/solver/test_iQR_wolf2.cpp
@@ -147,18 +147,18 @@ int main(int argc, char *argv[])
myScanner->loadAssimpModel(modelFileName);
//variables
- Eigen::Vector3s odom_reading;
- Eigen::Vector2s gps_fix_reading;
- Eigen::VectorXs pose_odom(3); //current odometry integred pose
- Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
- Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
- Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
+ Eigen::Vector3d odom_reading;
+ Eigen::Vector2d gps_fix_reading;
+ Eigen::VectorXd pose_odom(3); //current odometry integred pose
+ Eigen::VectorXd ground_truth(n_execution * 3); //all true poses
+ Eigen::VectorXd odom_trajectory(n_execution * 3); //open loop trajectory
+ Eigen::VectorXd mean_times = Eigen::VectorXd::Zero(7);
clock_t t1, t2;
// Wolf manager initialization
- Eigen::Vector3s odom_pose = Eigen::Vector3s::Zero();
- Eigen::Vector3s gps_pose = Eigen::Vector3s::Zero();
- Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
+ Eigen::Vector3d odom_pose = Eigen::Vector3d::Zero();
+ Eigen::Vector3d gps_pose = Eigen::Vector3d::Zero();
+ Eigen::Vector4d laser_1_pose, laser_2_pose; //xyz + theta
laser_1_pose << 1.2, 0, 0, 0; //laser 1
laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
SensorOdom2D odom_sensor(std::make_shared<StateBlock>(odom_pose.head(2)), std::make_shared<StateBlock>(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
@@ -171,8 +171,8 @@ int main(int argc, char *argv[])
ground_truth.head(3) = pose_odom;
odom_trajectory.head(3) = pose_odom;
- WolfManager* wolf_manager_QR = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
- WolfManager* wolf_manager_ceres = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
+ WolfManager* wolf_manager_QR = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution*10, 0.01);
+ WolfManager* wolf_manager_ceres = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution*10, 0.01);
// Ceres initialization
ceres::Solver::Options ceres_options;
@@ -242,10 +242,10 @@ int main(int argc, char *argv[])
// ADD CAPTURES ---------------------------
//std::cout << "ADD CAPTURES..." << std::endl;
// adding new sensor captures
- wolf_manager_QR->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
- wolf_manager_QR->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXs::Identity(3,3)));
- wolf_manager_ceres->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
- wolf_manager_ceres->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXs::Identity(3,3)));
+ wolf_manager_QR->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXd::Identity(2,2)));
+ wolf_manager_QR->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3,3)));
+ wolf_manager_ceres->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXd::Identity(2,2)));
+ wolf_manager_ceres->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3,3)));
//wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
//wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
// updating problem
@@ -364,7 +364,7 @@ int main(int argc, char *argv[])
// Vehicle poses
std::cout << "Vehicle poses..." << std::endl;
int i = 0;
- Eigen::VectorXs state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().size() * 3);
+ Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().size() * 3);
for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().end(); frame_it++)
{
if (complex_angle)
@@ -377,10 +377,10 @@ int main(int argc, char *argv[])
// Landmarks
std::cout << "Landmarks..." << std::endl;
i = 0;
- Eigen::VectorXs landmarks(wolf_manager_QR->getProblem()->getMap()->getLandmarkList().size() * 2);
+ Eigen::VectorXd landmarks(wolf_manager_QR->getProblem()->getMap()->getLandmarkList().size() * 2);
for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
{
- Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getP()->get());
+ Eigen::Map<Eigen::Vector2d> landmark((*landmark_it)->getP()->get());
landmarks.segment(i, 2) = landmark;
i += 2;
}
diff --git a/hello_wolf/capture_range_bearing.cpp b/hello_wolf/capture_range_bearing.cpp
index f2e3ad79dfac128288c65d127b47d4efcd599685..81322d73d2e592b0bf07db7100b75f6167b46020 100644
--- a/hello_wolf/capture_range_bearing.cpp
+++ b/hello_wolf/capture_range_bearing.cpp
@@ -10,7 +10,7 @@
namespace wolf
{
-CaptureRangeBearing::CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _bearings) :
+CaptureRangeBearing::CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings) :
CaptureBase("RANGE BEARING", _ts, _scanner),
ids_(_ids),
ranges_(_ranges),
diff --git a/hello_wolf/capture_range_bearing.h b/hello_wolf/capture_range_bearing.h
index 435726b2c876b30b48235ce112ffcab09d61af36..8bc32a93d452629b5c69358c5d751c070eeba1a6 100644
--- a/hello_wolf/capture_range_bearing.h
+++ b/hello_wolf/capture_range_bearing.h
@@ -20,22 +20,22 @@ using namespace Eigen;
class CaptureRangeBearing : public CaptureBase
{
public:
- CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXs& _ranges, const Eigen::VectorXs& _bearings);
+ CaptureRangeBearing(const TimeStamp& _ts, const SensorBasePtr& _scanner, const Eigen::VectorXi& _ids, const Eigen::VectorXd& _ranges, const Eigen::VectorXd& _bearings);
virtual ~CaptureRangeBearing();
const VectorXi& getIds () const;
const int& getId (int _i) const;
- const Eigen::VectorXs& getRanges () const;
- const Eigen::VectorXs& getBearings () const;
+ const Eigen::VectorXd& getRanges () const;
+ const Eigen::VectorXd& getBearings () const;
const double& getRange (int _i) const;
const double& getBearing (int _i) const;
- Eigen::Vector2s getRangeBearing (int _i) const;
+ Eigen::Vector2d getRangeBearing (int _i) const;
Eigen::Matrix<double, Dynamic, 2> getRangeBearing() const;
private:
VectorXi ids_; // identifiers
- VectorXs ranges_; // ranges
- VectorXs bearings_; // bearings
+ VectorXd ranges_; // ranges
+ VectorXd bearings_; // bearings
};
inline const Eigen::VectorXi& CaptureRangeBearing::getIds() const
@@ -48,12 +48,12 @@ inline const int& CaptureRangeBearing::getId(int _i) const
return ids_(_i);
}
-inline const Eigen::VectorXs& CaptureRangeBearing::getRanges() const
+inline const Eigen::VectorXd& CaptureRangeBearing::getRanges() const
{
return ranges_;
}
-inline const Eigen::VectorXs& CaptureRangeBearing::getBearings() const
+inline const Eigen::VectorXd& CaptureRangeBearing::getBearings() const
{
return bearings_;
}
@@ -73,9 +73,9 @@ inline Eigen::Matrix<double,Dynamic,2> CaptureRangeBearing::getRangeBearing() co
return (Matrix<double,Dynamic,2>() << ranges_, bearings_).finished();
}
-inline Eigen::Vector2s CaptureRangeBearing::getRangeBearing(int _i) const
+inline Eigen::Vector2d CaptureRangeBearing::getRangeBearing(int _i) const
{
- return Vector2s(ranges_(_i), bearings_(_i));
+ return Vector2d(ranges_(_i), bearings_(_i));
}
} /* namespace wolf */
diff --git a/hello_wolf/feature_range_bearing.cpp b/hello_wolf/feature_range_bearing.cpp
index 3be9365676824e14689e63a6f969f33123736fb6..40967163a3a5ab643cd18fc068f3c0ab55a2b1d9 100644
--- a/hello_wolf/feature_range_bearing.cpp
+++ b/hello_wolf/feature_range_bearing.cpp
@@ -10,7 +10,7 @@
namespace wolf
{
-FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
+FeatureRangeBearing::FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
FeatureBase("RANGE BEARING", _measurement, _meas_covariance)
{
//
diff --git a/hello_wolf/feature_range_bearing.h b/hello_wolf/feature_range_bearing.h
index b68419434b320aa3c2352e833419cb33dbad14aa..6a8dada9cd8bb9f53f8a4386a687c4655c8d14bf 100644
--- a/hello_wolf/feature_range_bearing.h
+++ b/hello_wolf/feature_range_bearing.h
@@ -18,7 +18,7 @@ WOLF_PTR_TYPEDEFS(FeatureRangeBearing);
class FeatureRangeBearing : public FeatureBase
{
public:
- FeatureRangeBearing(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
+ FeatureRangeBearing(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
virtual ~FeatureRangeBearing();
};
diff --git a/hello_wolf/hello_wolf.cpp b/hello_wolf/hello_wolf.cpp
index 4c1aa9ab06660e053f6bdd1b922ca976b52c92a7..0ef464aaae9430ee838dcd0bdcc140ff7dc83220 100644
--- a/hello_wolf/hello_wolf.cpp
+++ b/hello_wolf/hello_wolf.cpp
@@ -112,7 +112,7 @@ int main()
// sensor odometer 2D
IntrinsicsOdom2DPtr intrinsics_odo = std::make_shared<IntrinsicsOdom2D>();
- SensorBasePtr sensor_odo = problem->installSensor("ODOM 2D", "sensor odo", Vector3s(0,0,0), intrinsics_odo);
+ SensorBasePtr sensor_odo = problem->installSensor("ODOM 2D", "sensor odo", Vector3d(0,0,0), intrinsics_odo);
// processor odometer 2D
ProcessorParamsOdom2DPtr params_odo = std::make_shared<ProcessorParamsOdom2D>();
@@ -129,7 +129,7 @@ int main()
IntrinsicsRangeBearingPtr intrinsics_rb = std::make_shared<IntrinsicsRangeBearing>();
intrinsics_rb->noise_range_metres_std = 0.1;
intrinsics_rb->noise_bearing_degrees_std = 1.0;
- SensorBasePtr sensor_rb = problem->installSensor("RANGE BEARING", "sensor RB", Vector3s(1,1,0), intrinsics_rb);
+ SensorBasePtr sensor_rb = problem->installSensor("RANGE BEARING", "sensor RB", Vector3d(1,1,0), intrinsics_rb);
// processor Range and Bearing
ProcessorParamsRangeBearingPtr params_rb = std::make_shared<ProcessorParamsRangeBearing>();
@@ -139,8 +139,8 @@ int main()
// initialize
TimeStamp t(0.0); // t : 0.0
- Vector3s x(0,0,0);
- Matrix3s P = Matrix3s::Identity() * 0.1;
+ Vector3d x(0,0,0);
+ Matrix3d P = Matrix3d::Identity() * 0.1;
problem->setPrior(x, P, t, 0.5); // KF1 : (0,0,0)
// SELF CALIBRATION ===================================================
@@ -156,12 +156,12 @@ int main()
// CONFIGURE ==========================================================
// Motion data
- Vector2s motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
- Matrix2s motion_cov = 0.1 * Matrix2s::Identity();
+ Vector2d motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
+ Matrix2d motion_cov = 0.1 * Matrix2d::Identity();
// landmark observations data
VectorXi ids;
- VectorXs ranges, bearings;
+ VectorXd ranges, bearings;
// SET OF EVENTS =======================================================
std::cout << std::endl;
@@ -223,16 +223,16 @@ int main()
for (auto& sen : problem->getHardware()->getSensorList())
for (auto& sb : sen->getStateBlockVec())
if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+ sb->setState(sb->getState() + VectorXd::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto& kf : problem->getTrajectory()->getFrameList())
if (kf->isKeyOrAux())
for (auto& sb : kf->getStateBlockVec())
if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+ sb->setState(sb->getState() + VectorXd::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto& lmk : problem->getMap()->getLandmarkList())
for (auto& sb : lmk->getStateBlockVec())
if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+ sb->setState(sb->getState() + VectorXd::Random(sb->getSize()) * 0.5); // We perturb A LOT !
problem->print(1,0,1,0);
// SOLVE again
@@ -247,13 +247,13 @@ int main()
for (auto& kf : problem->getTrajectory()->getFrameList())
if (kf->isKeyOrAux())
{
- Eigen::MatrixXs cov;
+ Eigen::MatrixXd cov;
kf->getCovariance(cov);
WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
}
for (auto& lmk : problem->getMap()->getLandmarkList())
{
- Eigen::MatrixXs cov;
+ Eigen::MatrixXd cov;
lmk->getCovariance(cov);
WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
}
diff --git a/hello_wolf/hello_wolf_autoconf.cpp b/hello_wolf/hello_wolf_autoconf.cpp
index de8aa4fb7488c8f8b78c703d0b8f363094b97a23..8bc19c74ac118c4acd00a8c4648ea64680535351 100644
--- a/hello_wolf/hello_wolf_autoconf.cpp
+++ b/hello_wolf/hello_wolf_autoconf.cpp
@@ -136,12 +136,12 @@ int main()
// CONFIGURE input data (motion and measurements) ==============================================
// Motion data
- Vector2s motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
- Matrix2s motion_cov = 0.1 * Matrix2s::Identity();
+ Vector2d motion_data(1.0, 0.0); // Will advance 1m at each keyframe, will not turn.
+ Matrix2d motion_cov = 0.1 * Matrix2d::Identity();
// landmark observations data
VectorXi ids;
- VectorXs ranges, bearings;
+ VectorXd ranges, bearings;
// SET OF EVENTS: make things happen =======================================================
std::cout << std::endl;
@@ -206,16 +206,16 @@ int main()
for (auto& sen : problem->getHardware()->getSensorList())
for (auto& sb : sen->getStateBlockVec())
if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+ sb->setState(sb->getState() + VectorXd::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto& kf : problem->getTrajectory()->getFrameList())
if (kf->isKeyOrAux())
for (auto& sb : kf->getStateBlockVec())
if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+ sb->setState(sb->getState() + VectorXd::Random(sb->getSize()) * 0.5); // We perturb A LOT !
for (auto& lmk : problem->getMap()->getLandmarkList())
for (auto& sb : lmk->getStateBlockVec())
if (sb && !sb->isFixed())
- sb->setState(sb->getState() + VectorXs::Random(sb->getSize()) * 0.5); // We perturb A LOT !
+ sb->setState(sb->getState() + VectorXd::Random(sb->getSize()) * 0.5); // We perturb A LOT !
problem->print(1,0,1,0);
// SOLVE again
@@ -230,13 +230,13 @@ int main()
for (auto& kf : problem->getTrajectory()->getFrameList())
if (kf->isKeyOrAux())
{
- Eigen::MatrixXs cov;
+ Eigen::MatrixXd cov;
kf->getCovariance(cov);
WOLF_TRACE("KF", kf->id(), "_cov = \n", cov);
}
for (auto& lmk : problem->getMap()->getLandmarkList())
{
- Eigen::MatrixXs cov;
+ Eigen::MatrixXd cov;
lmk->getCovariance(cov);
WOLF_TRACE("L", lmk->id(), "_cov = \n", cov);
}
diff --git a/hello_wolf/landmark_point_2D.cpp b/hello_wolf/landmark_point_2D.cpp
index 2b5fac1b49d674fa7fd3ba8c8883939e4035880a..2e74de9065542dde024989342087fa8dbdcee659 100644
--- a/hello_wolf/landmark_point_2D.cpp
+++ b/hello_wolf/landmark_point_2D.cpp
@@ -10,7 +10,7 @@
namespace wolf
{
-LandmarkPoint2D::LandmarkPoint2D(int _id, const Eigen::Vector2s& _xy) :
+LandmarkPoint2D::LandmarkPoint2D(int _id, const Eigen::Vector2d& _xy) :
LandmarkBase("POINT 2D", std::make_shared<StateBlock>(_xy))
{
setId(_id);
diff --git a/hello_wolf/landmark_point_2D.h b/hello_wolf/landmark_point_2D.h
index 0d7fe13a00f5112f00b0161350d872a6f59895b2..371db93ee80e991b2981307a97b6a6a54d466491 100644
--- a/hello_wolf/landmark_point_2D.h
+++ b/hello_wolf/landmark_point_2D.h
@@ -18,7 +18,7 @@ WOLF_PTR_TYPEDEFS(LandmarkPoint2D);
class LandmarkPoint2D : public LandmarkBase
{
public:
- LandmarkPoint2D(int _id, const Eigen::Vector2s& _xy);
+ LandmarkPoint2D(int _id, const Eigen::Vector2d& _xy);
virtual ~LandmarkPoint2D();
};
diff --git a/hello_wolf/processor_range_bearing.cpp b/hello_wolf/processor_range_bearing.cpp
index a7d5489b6a6924deefa39f5334ec8cb06a67236a..ca3f56c3ccef6052be4dc89301334c4eed7104ef 100644
--- a/hello_wolf/processor_range_bearing.cpp
+++ b/hello_wolf/processor_range_bearing.cpp
@@ -82,7 +82,7 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
}
// 5. create feature
- Vector2s rb(range,bearing);
+ Vector2d rb(range,bearing);
auto ftr = FeatureBase::emplace<FeatureRangeBearing>(capture_rb,
rb,
getSensor()->getNoiseCov());
@@ -100,53 +100,53 @@ void ProcessorRangeBearing::processCapture(CaptureBasePtr _capture)
}
-Eigen::Vector2s ProcessorRangeBearing::observe(const Eigen::Vector2s& lmk_w) const
+Eigen::Vector2d ProcessorRangeBearing::observe(const Eigen::Vector2d& lmk_w) const
{
return polar(toSensor(lmk_w));
}
-Eigen::Vector2s ProcessorRangeBearing::invObserve(double r, double b) const
+Eigen::Vector2d ProcessorRangeBearing::invObserve(double r, double b) const
{
return fromSensor(rect(r, b));
}
-ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector3s& _pose) const
+ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector3d& _pose) const
{
Trf H = Eigen::Translation<double,2>(_pose(0), _pose(1)) * Eigen::Rotation2D<double>(_pose(2)) ;
return H;
}
-ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2s& _position,
- const Eigen::Vector1s& _orientation) const
+ProcessorRangeBearing::Trf ProcessorRangeBearing::transform(const Eigen::Vector2d& _position,
+ const Eigen::Vector1d& _orientation) const
{
Trf H = Eigen::Translation<double,2>(_position(0), _position(1)) * Eigen::Rotation2D<double>(_orientation(0)) ;
return H;
}
-Eigen::Vector2s ProcessorRangeBearing::fromSensor(const Eigen::Vector2s& lmk_s) const
+Eigen::Vector2d ProcessorRangeBearing::fromSensor(const Eigen::Vector2d& lmk_s) const
{
Trf H_w_r = transform(getProblem()->getCurrentState());
return H_w_r * H_r_s * lmk_s;
}
-Eigen::Vector2s ProcessorRangeBearing::toSensor(const Eigen::Vector2s& lmk_w) const
+Eigen::Vector2d ProcessorRangeBearing::toSensor(const Eigen::Vector2d& lmk_w) const
{
// Trf H_w_r = transform(getSensor()->getP()->getState(), getSensor()->getO()->getState());
Trf H_w_r = transform(getProblem()->getCurrentState());
return (H_w_r * H_r_s).inverse() * lmk_w;
}
-Eigen::Vector2s ProcessorRangeBearing::polar(const Eigen::Vector2s& rect) const
+Eigen::Vector2d ProcessorRangeBearing::polar(const Eigen::Vector2d& rect) const
{
- Eigen::Vector2s polar;
+ Eigen::Vector2d polar;
polar(0) = rect.norm();
polar(1) = atan2(rect(1), rect(0));
return polar;
}
-Eigen::Vector2s ProcessorRangeBearing::rect(double range, double bearing) const
+Eigen::Vector2d ProcessorRangeBearing::rect(double range, double bearing) const
{
- return range * (Vector2s() << cos(bearing), sin(bearing)).finished();
+ return range * (Vector2d() << cos(bearing), sin(bearing)).finished();
}
bool ProcessorRangeBearing::storeKeyFrame(FrameBasePtr _frame_ptr)
diff --git a/hello_wolf/processor_range_bearing.h b/hello_wolf/processor_range_bearing.h
index 59ff14d83ac6c061d809020bf8b2ec7eef27ab62..ec1d8619835f1169a192639db09d8be93087cdae 100644
--- a/hello_wolf/processor_range_bearing.h
+++ b/hello_wolf/processor_range_bearing.h
@@ -79,16 +79,16 @@ class ProcessorRangeBearing : public ProcessorBase
protected:
// helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h
- Eigen::Vector2s observe (const Eigen::Vector2s& lmk_w) const;
- Eigen::Vector2s invObserve (double r, double b) const;
+ Eigen::Vector2d observe (const Eigen::Vector2d& lmk_w) const;
+ Eigen::Vector2d invObserve (double r, double b) const;
private:
// helper methods -- to be used only here -- they would be better off in a separate library e.g. range_bearing_tools.h
- Trf transform (const Eigen::Vector3s& _pose) const;
- Trf transform (const Eigen::Vector2s& _position, const Eigen::Vector1s& _orientation) const;
- Eigen::Vector2s fromSensor (const Eigen::Vector2s& lmk_s) const;
- Eigen::Vector2s toSensor (const Eigen::Vector2s& lmk_w) const;
- Eigen::Vector2s polar (const Eigen::Vector2s& rect) const;
- Eigen::Vector2s rect (double range, double bearing) const;
+ Trf transform (const Eigen::Vector3d& _pose) const;
+ Trf transform (const Eigen::Vector2d& _position, const Eigen::Vector1d& _orientation) const;
+ Eigen::Vector2d fromSensor (const Eigen::Vector2d& lmk_s) const;
+ Eigen::Vector2d toSensor (const Eigen::Vector2d& lmk_w) const;
+ Eigen::Vector2d polar (const Eigen::Vector2d& rect) const;
+ Eigen::Vector2d rect (double range, double bearing) const;
};
inline void ProcessorRangeBearing::configure(SensorBasePtr _sensor)
diff --git a/hello_wolf/sensor_range_bearing.cpp b/hello_wolf/sensor_range_bearing.cpp
index a63a7a248f739294a17a533dec300c11a941ee84..3294c7f7fcba9afc9ac499815ee5a28820b98335 100644
--- a/hello_wolf/sensor_range_bearing.cpp
+++ b/hello_wolf/sensor_range_bearing.cpp
@@ -13,7 +13,7 @@ namespace wolf
WOLF_PTR_TYPEDEFS(SensorRangeBearing);
-SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const Eigen::Vector2s& _noise_std) :
+SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std) :
SensorBase("RANGE BEARING",
std::make_shared<StateBlock>(_extrinsics.head<2>(), true),
std::make_shared<StateAngle>(_extrinsics(2), true),
@@ -23,8 +23,8 @@ SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const
assert(_extrinsics.size() == 3 && "Bad extrinsics vector size. Must be 3 for 2D");
}
-SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const IntrinsicsRangeBearingPtr _intr) :
- SensorRangeBearing(_extrinsics, Eigen::Vector2s(_intr->noise_range_metres_std, toRad(_intr->noise_bearing_degrees_std)))
+SensorRangeBearing::SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const IntrinsicsRangeBearingPtr _intr) :
+ SensorRangeBearing(_extrinsics, Eigen::Vector2d(_intr->noise_range_metres_std, toRad(_intr->noise_bearing_degrees_std)))
{
//
}
diff --git a/hello_wolf/sensor_range_bearing.h b/hello_wolf/sensor_range_bearing.h
index 8e279bfdbcffa8f710df8f07b08d8d7c794258d2..75f9a3d8a9b66f096ab8bbc2085ca271efeb04fd 100644
--- a/hello_wolf/sensor_range_bearing.h
+++ b/hello_wolf/sensor_range_bearing.h
@@ -42,8 +42,8 @@ WOLF_PTR_TYPEDEFS(SensorRangeBearing)
class SensorRangeBearing : public SensorBase
{
public:
- SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const Eigen::Vector2s& _noise_std);
- SensorRangeBearing(const Eigen::VectorXs& _extrinsics, const IntrinsicsRangeBearingPtr _intr);
+ SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const Eigen::Vector2d& _noise_std);
+ SensorRangeBearing(const Eigen::VectorXd& _extrinsics, const IntrinsicsRangeBearingPtr _intr);
WOLF_SENSOR_CREATE(SensorRangeBearing, IntrinsicsRangeBearing, 3);
virtual ~SensorRangeBearing();
diff --git a/include/core/capture/capture_base.h b/include/core/capture/capture_base.h
index c660f7766163e8c9957e4663964dbda2dc5ec692..822e275185fbcfcae0b68bdbd70d6d200e5439b5 100644
--- a/include/core/capture/capture_base.h
+++ b/include/core/capture/capture_base.h
@@ -102,8 +102,8 @@ class CaptureBase : public NodeBase, public std::enable_shared_from_this<Capture
bool hasCalibration() {return calib_size_ > 0;}
SizeEigen getCalibSize() const;
- virtual Eigen::VectorXs getCalibration() const;
- void setCalibration(const Eigen::VectorXs& _calib);
+ virtual Eigen::VectorXd getCalibration() const;
+ void setCalibration(const Eigen::VectorXd& _calib);
void move(FrameBasePtr);
void link(FrameBasePtr);
template<typename classType, typename... T>
diff --git a/include/core/capture/capture_diff_drive.h b/include/core/capture/capture_diff_drive.h
index 0f785cdd3d8178a5fb5b194841e76d46547f58cf..2300a65b2a203ccc6bb9ca155fab6dbb6b2bdd96 100644
--- a/include/core/capture/capture_diff_drive.h
+++ b/include/core/capture/capture_diff_drive.h
@@ -30,8 +30,8 @@ public:
**/
CaptureDiffDrive(const TimeStamp& _ts,
const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr,
StateBlockPtr _p_ptr = nullptr,
StateBlockPtr _o_ptr = nullptr,
@@ -39,7 +39,7 @@ public:
virtual ~CaptureDiffDrive() = default;
- virtual Eigen::VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const override;
+ virtual Eigen::VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
};
diff --git a/include/core/capture/capture_motion.h b/include/core/capture/capture_motion.h
index 7fa7e38eb1dbad2579222c2dff247b9614322594..d78cd5169f3dfc523c9435c11dc65b41ef6b3591 100644
--- a/include/core/capture/capture_motion.h
+++ b/include/core/capture/capture_motion.h
@@ -49,7 +49,7 @@ class CaptureMotion : public CaptureBase
CaptureMotion(const std::string & _type,
const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
+ const Eigen::VectorXd& _data,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr);
@@ -57,8 +57,8 @@ class CaptureMotion : public CaptureBase
CaptureMotion(const std::string & _type,
const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr,
@@ -72,29 +72,29 @@ class CaptureMotion : public CaptureBase
virtual bool isMotion() const override { return true; }
// Data
- const Eigen::VectorXs& getData() const;
- const Eigen::MatrixXs& getDataCovariance() const;
- void setData(const Eigen::VectorXs& _data);
- void setDataCovariance(const Eigen::MatrixXs& _data_cov);
+ const Eigen::VectorXd& getData() const;
+ const Eigen::MatrixXd& getDataCovariance() const;
+ void setData(const Eigen::VectorXd& _data);
+ void setDataCovariance(const Eigen::MatrixXd& _data_cov);
// Buffer
MotionBuffer& getBuffer();
const MotionBuffer& getBuffer() const;
// Buffer's initial conditions for pre-integration
- VectorXs getCalibrationPreint() const;
- void setCalibrationPreint(const VectorXs& _calib_preint);
- MatrixXs getJacobianCalib() const;
- MatrixXs getJacobianCalib(const TimeStamp& _ts) const;
+ VectorXd getCalibrationPreint() const;
+ void setCalibrationPreint(const VectorXd& _calib_preint);
+ MatrixXd getJacobianCalib() const;
+ MatrixXd getJacobianCalib(const TimeStamp& _ts) const;
// Get delta preintegrated, and corrected for changes on calibration params
- VectorXs getDeltaCorrected(const VectorXs& _calib_current) const;
- VectorXs getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts) const;
- VectorXs getDeltaPreint() const;
- VectorXs getDeltaPreint(const TimeStamp& _ts) const;
- MatrixXs getDeltaPreintCov() const;
- MatrixXs getDeltaPreintCov(const TimeStamp& _ts) const;
- virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const;
+ VectorXd getDeltaCorrected(const VectorXd& _calib_current) const;
+ VectorXd getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const;
+ VectorXd getDeltaPreint() const;
+ VectorXd getDeltaPreint(const TimeStamp& _ts) const;
+ MatrixXd getDeltaPreintCov() const;
+ MatrixXd getDeltaPreintCov(const TimeStamp& _ts) const;
+ virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const;
// Origin frame and capture
CaptureBasePtr getOriginCapture();
@@ -102,30 +102,30 @@ class CaptureMotion : public CaptureBase
// member data:
private:
- Eigen::VectorXs data_; ///< Motion data in form of vector mandatory
- Eigen::MatrixXs data_cov_; ///< Motion data covariance
- Eigen::VectorXs calib_preint_; ///< Calibration parameters used during pre-integration
+ Eigen::VectorXd data_; ///< Motion data in form of vector mandatory
+ Eigen::MatrixXd data_cov_; ///< Motion data covariance
+ Eigen::VectorXd calib_preint_; ///< Calibration parameters used during pre-integration
MotionBuffer buffer_; ///< Buffer of motions between this Capture and the next one.
CaptureBaseWPtr capture_origin_ptr_; ///< Pointer to the origin capture of the motion
};
-inline const Eigen::VectorXs& CaptureMotion::getData() const
+inline const Eigen::VectorXd& CaptureMotion::getData() const
{
return data_;
}
-inline const Eigen::MatrixXs& CaptureMotion::getDataCovariance() const
+inline const Eigen::MatrixXd& CaptureMotion::getDataCovariance() const
{
return data_cov_;
}
-inline void CaptureMotion::setData(const Eigen::VectorXs& _data)
+inline void CaptureMotion::setData(const Eigen::VectorXd& _data)
{
assert(_data.size() == data_.size() && "Wrong size of data vector!");
data_ = _data;
}
-inline void CaptureMotion::setDataCovariance(const Eigen::MatrixXs& _data_cov)
+inline void CaptureMotion::setDataCovariance(const Eigen::MatrixXd& _data_cov)
{
assert(_data_cov.rows() == data_cov_.rows() && "Wrong number of rows of data vector!");
assert(_data_cov.cols() == data_cov_.cols() && "Wrong number of cols of data vector!");
@@ -142,17 +142,17 @@ inline MotionBuffer& CaptureMotion::getBuffer()
return buffer_;
}
-inline Eigen::MatrixXs CaptureMotion::getJacobianCalib() const
+inline Eigen::MatrixXd CaptureMotion::getJacobianCalib() const
{
return getBuffer().get().back().jacobian_calib_;
}
-inline Eigen::MatrixXs CaptureMotion::getJacobianCalib(const TimeStamp& _ts) const
+inline Eigen::MatrixXd CaptureMotion::getJacobianCalib(const TimeStamp& _ts) const
{
return getBuffer().getMotion(_ts).jacobian_calib_;
}
-inline Eigen::VectorXs CaptureMotion::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const
+inline Eigen::VectorXd CaptureMotion::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
{
WOLF_DEBUG("WARNING: using Cartesian sum for delta correction. \nIf your deltas lie on a manifold, derive this function and implement the proper delta correction!")
return _delta + _delta_error;
@@ -168,32 +168,32 @@ inline void CaptureMotion::setOriginCapture(CaptureBasePtr _capture_origin_ptr)
capture_origin_ptr_ = _capture_origin_ptr;
}
-inline VectorXs CaptureMotion::getCalibrationPreint() const
+inline VectorXd CaptureMotion::getCalibrationPreint() const
{
return calib_preint_;
}
-inline void CaptureMotion::setCalibrationPreint(const VectorXs& _calib_preint)
+inline void CaptureMotion::setCalibrationPreint(const VectorXd& _calib_preint)
{
calib_preint_ = _calib_preint;
}
-inline VectorXs CaptureMotion::getDeltaPreint() const
+inline VectorXd CaptureMotion::getDeltaPreint() const
{
return getBuffer().get().back().delta_integr_;
}
-inline VectorXs CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const
+inline VectorXd CaptureMotion::getDeltaPreint(const TimeStamp& _ts) const
{
return getBuffer().getMotion(_ts).delta_integr_;
}
-inline MatrixXs CaptureMotion::getDeltaPreintCov() const
+inline MatrixXd CaptureMotion::getDeltaPreintCov() const
{
return getBuffer().get().back().delta_integr_cov_;
}
-inline MatrixXs CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const
+inline MatrixXd CaptureMotion::getDeltaPreintCov(const TimeStamp& _ts) const
{
return getBuffer().getMotion(_ts).delta_integr_cov_;
}
diff --git a/include/core/capture/capture_odom_2D.h b/include/core/capture/capture_odom_2D.h
index 06a507aa8368474460e93b7b6d64c059180d41d4..ac242ee1ec64c81c82787b9a383f7ffa7d7444d8 100644
--- a/include/core/capture/capture_odom_2D.h
+++ b/include/core/capture/capture_odom_2D.h
@@ -22,24 +22,24 @@ class CaptureOdom2D : public CaptureMotion
public:
CaptureOdom2D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
+ const Eigen::VectorXd& _data,
CaptureBasePtr _capture_origin_ptr = nullptr);
CaptureOdom2D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr = nullptr);
virtual ~CaptureOdom2D();
- virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const override;
+ virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
};
-inline Eigen::VectorXs CaptureOdom2D::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const
+inline Eigen::VectorXd CaptureOdom2D::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
{
- Vector3s delta = _delta + _delta_error;
+ Vector3d delta = _delta + _delta_error;
delta(2) = pi2pi(delta(2));
return delta;
}
diff --git a/include/core/capture/capture_odom_3D.h b/include/core/capture/capture_odom_3D.h
index cc810448af0a68d7287483b0afe538bda0e0d2b2..c9990f38513c63809d7cb396790ba388ef0d5383 100644
--- a/include/core/capture/capture_odom_3D.h
+++ b/include/core/capture/capture_odom_3D.h
@@ -22,18 +22,18 @@ class CaptureOdom3D : public CaptureMotion
public:
CaptureOdom3D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
+ const Eigen::VectorXd& _data,
CaptureBasePtr _capture_origin_ptr = nullptr);
CaptureOdom3D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr = nullptr);
virtual ~CaptureOdom3D();
- virtual VectorXs correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const override;
+ virtual VectorXd correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const override;
};
diff --git a/include/core/capture/capture_pose.h b/include/core/capture/capture_pose.h
index aff48d40a25903ccc06a633282467ab5d2639459..4073cb1e2681b4024029146d83372961085db646 100644
--- a/include/core/capture/capture_pose.h
+++ b/include/core/capture/capture_pose.h
@@ -18,12 +18,12 @@ WOLF_PTR_TYPEDEFS(CapturePose);
class CapturePose : public CaptureBase
{
protected:
- Eigen::VectorXs data_; ///< Raw data.
- Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
+ Eigen::VectorXd data_; ///< Raw data.
+ Eigen::MatrixXd data_covariance_; ///< Noise of the capture.
public:
- CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
+ CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_covariance);
virtual ~CapturePose();
diff --git a/include/core/capture/capture_velocity.h b/include/core/capture/capture_velocity.h
index f665c181311494bf0bbc154b3f1e030c7f1ecad2..d2d1c686dfd84c0d8d9ca3c4fbe0a1b7d17091e1 100644
--- a/include/core/capture/capture_velocity.h
+++ b/include/core/capture/capture_velocity.h
@@ -33,14 +33,14 @@ public:
**/
CaptureVelocity(const TimeStamp& _ts,
const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _velocity,
+ const Eigen::VectorXd& _velocity,
SizeEigen _delta_size, SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr);
CaptureVelocity(const TimeStamp& _ts,
const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _velocity,
- const Eigen::MatrixXs& _velocity_cov,
+ const Eigen::VectorXd& _velocity,
+ const Eigen::MatrixXd& _velocity_cov,
SizeEigen _delta_size, SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr,
StateBlockPtr _p_ptr = nullptr,
@@ -49,9 +49,9 @@ public:
virtual ~CaptureVelocity() = default;
- const Eigen::VectorXs& getVelocity() const;
+ const Eigen::VectorXd& getVelocity() const;
- const Eigen::MatrixXs& getVelocityCov() const;
+ const Eigen::MatrixXd& getVelocityCov() const;
};
} // namespace wolf
diff --git a/include/core/ceres_wrapper/qr_manager.h b/include/core/ceres_wrapper/qr_manager.h
index 5c834b404592d72913abb20b95a40679b0f83e4a..691c44166da222b893c1d8acd428c7e63da71539 100644
--- a/include/core/ceres_wrapper/qr_manager.h
+++ b/include/core/ceres_wrapper/qr_manager.h
@@ -17,10 +17,10 @@ namespace wolf
class QRManager : public SolverManager
{
protected:
- Eigen::SparseQR<Eigen::SparseMatrixs, Eigen::COLAMDOrdering<int>> solver_;
- Eigen::SparseQR<Eigen::SparseMatrixs, Eigen::NaturalOrdering<int>> covariance_solver_;
- Eigen::SparseMatrixs A_;
- Eigen::VectorXs b_;
+ Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::COLAMDOrdering<int>> solver_;
+ Eigen::SparseQR<Eigen::SparseMatrixd, Eigen::NaturalOrdering<int>> covariance_solver_;
+ Eigen::SparseMatrixd A_;
+ Eigen::VectorXd b_;
std::map<StateBlockPtr, unsigned int> sb_2_col_;
std::map<FactorBasePtr, unsigned int> fac_2_row_;
bool any_state_block_removed_;
diff --git a/include/core/ceres_wrapper/sparse_utils.h b/include/core/ceres_wrapper/sparse_utils.h
index 187d7e21043b5ff57f23d223b04d17bda4c073e6..4b7171f77eaf09cc902ae24aea7dd49c5a4adfca 100644
--- a/include/core/ceres_wrapper/sparse_utils.h
+++ b/include/core/ceres_wrapper/sparse_utils.h
@@ -16,7 +16,7 @@ namespace wolf
void eraseBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const unsigned int& _row, const unsigned int& _n_rows)
{
- A.middleRows(_row,_n_rows) = Eigen::SparseMatrixs(_n_rows,A.cols());
+ A.middleRows(_row,_n_rows) = Eigen::SparseMatrixd(_n_rows,A.cols());
A.makeCompressed();
}
@@ -28,7 +28,7 @@ void eraseBlockRow(Eigen::SparseMatrix<double, Eigen::ColMajor>& A, const unsign
void eraseBlockCol(Eigen::SparseMatrix<double, Eigen::ColMajor>& A, const unsigned int& _col, const unsigned int& _n_cols)
{
- A.middleCols(_col,_n_cols) = Eigen::SparseMatrixs(A.rows(),_n_cols);
+ A.middleCols(_col,_n_cols) = Eigen::SparseMatrixd(A.rows(),_n_cols);
A.makeCompressed();
}
@@ -39,7 +39,7 @@ void eraseBlockCol(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const unsign
}
template<int _Options, typename _StorageIndex>
-void assignSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col)
+void assignSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col)
{
for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
@@ -49,7 +49,7 @@ void assignSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrix<double,_O
}
template<int _Options, typename _StorageIndex>
-void addSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col)
+void addSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col)
{
for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
@@ -59,7 +59,7 @@ void addSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrix<double,_Opti
}
template<int _Options, typename _StorageIndex>
-void insertSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col)
+void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrix<double,_Options,_StorageIndex>& original, const unsigned int& row, const unsigned int& col)
{
for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
@@ -75,15 +75,15 @@ void assignBlockRow(Eigen::SparseMatrix<double, Eigen::RowMajor>& A, const Eigen
A.makeCompressed();
}
-Eigen::SparseMatrixs createBlockDiagonal(const std::vector<Eigen::MatrixXs>& _diag_blocs)
+Eigen::SparseMatrixd createBlockDiagonal(const std::vector<Eigen::MatrixXd>& _diag_blocs)
{
unsigned int dim = _diag_blocs.front().rows();
unsigned int size = dim * _diag_blocs.size();
- Eigen::SparseMatrixs M(size,size);
+ Eigen::SparseMatrixd M(size,size);
unsigned int pos = 0;
- for (const Eigen::MatrixXs& omega_k : _diag_blocs)
+ for (const Eigen::MatrixXd& omega_k : _diag_blocs)
{
insertSparseBlock(omega_k, M, pos, pos);
pos += dim;
@@ -95,12 +95,12 @@ Eigen::SparseMatrixs createBlockDiagonal(const std::vector<Eigen::MatrixXs>& _di
}
template<int _Options, typename _StorageIndex>
-void getDiagonalBlocks(const Eigen::SparseMatrix<double,_Options,_StorageIndex>& _M, std::vector<Eigen::MatrixXs>& diag_, const unsigned int& dim)
+void getDiagonalBlocks(const Eigen::SparseMatrix<double,_Options,_StorageIndex>& _M, std::vector<Eigen::MatrixXd>& diag_, const unsigned int& dim)
{
assert(_M.rows() % dim == 0 && "number of rows must be multiple of dimension");
diag_.clear();
for (auto i = 0; i < _M.rows(); i += dim)
- diag_.push_back(Eigen::MatrixXs(_M.block(i,i,dim,dim)));
+ diag_.push_back(Eigen::MatrixXd(_M.block(i,i,dim,dim)));
}
} // namespace wolf
diff --git a/include/core/common/factory.h b/include/core/common/factory.h
index eda43efdcb646630164ebda2fb48002b5b3b0f62..cb19d7a67db300183bd3579b9c9acc2cdd31524a 100644
--- a/include/core/common/factory.h
+++ b/include/core/common/factory.h
@@ -190,10 +190,10 @@ namespace wolf
* bool first_defined = _lmk_node["first_defined"].as<bool>();
* bool last_defined = _lmk_node["last_defined"].as<bool>();
* unsigned int npoints = _lmk_node["points"].size();
- * Eigen::MatrixXs points(2,npoints);
+ * Eigen::MatrixXd points(2,npoints);
* for (unsigned int i = 0; i < npoints; i++)
* {
- * points.col(i) = _lmk_node["points"][i].as<Eigen::Vector2s>();
+ * points.col(i) = _lmk_node["points"][i].as<Eigen::Vector2d>();
* }
*
* // Create a new landmark
diff --git a/include/core/common/wolf.h b/include/core/common/wolf.h
index 583d7789da8db59bcfd04d30e7f0a060f98ab9cd..42f8fcb325b3eb176193d29d8df2a1fa3c4ac7f4 100644
--- a/include/core/common/wolf.h
+++ b/include/core/common/wolf.h
@@ -76,55 +76,25 @@ const double EPS_SMALL = 1e-16;
* The appended letter indicating this is 's', so that we have, e.g.,
* - VectorXf Vector of floats - defined by Eigen
* - VectorXd Vector of doubles - defined by Eigen
- * - VectorXs Vector of either double of float, depending on the type \b double, defined by Wolf.
+ * - VectorXd Vector of either double of float, depending on the type \b double, defined by Wolf.
*
*/
namespace Eigen // Eigen namespace extension
{
// 1. Vectors and Matrices
-typedef Matrix<double, 1, 1, RowMajor> Matrix1s; ///< 2x2 matrix of real double type
-typedef Matrix<double, 2, 2, RowMajor> Matrix2s; ///< 2x2 matrix of real double type
-typedef Matrix<double, 3, 3, RowMajor> Matrix3s; ///< 3x3 matrix of real double type
-typedef Matrix<double, 4, 4, RowMajor> Matrix4s; ///< 4x4 matrix of real double type
-typedef Matrix<double, 6, 6, RowMajor> Matrix6s; ///< 6x6 matrix of real double type
-typedef Matrix<double, 7, 7, RowMajor> Matrix7s; ///< 7x7 matrix of real double type
-typedef Matrix<double, Dynamic, Dynamic, RowMajor> MatrixXs; ///< variable size matrix of real double type
-typedef Matrix<double, 1, 1> Vector1s; ///< 1-vector of real double type
-typedef Matrix<double, 2, 1> Vector2s; ///< 2-vector of real double type
-typedef Matrix<double, 3, 1> Vector3s; ///< 3-vector of real double type
-typedef Matrix<double, 4, 1> Vector4s; ///< 4-vector of real double type
-typedef Matrix<double, 5, 1> Vector5s; ///< 5-vector of real double type
-typedef Matrix<double, 6, 1> Vector6s; ///< 6-vector of real double type
-typedef Matrix<double, 7, 1> Vector7s; ///< 7-vector of real double type
-typedef Matrix<double, 8, 1> Vector8s; ///< 8-vector of real double type
-typedef Matrix<double, 9, 1> Vector9s; ///< 9-vector of real double type
-typedef Matrix<double, 10, 1> Vector10s; ///< 10-vector of real double type
-typedef Matrix<double, Dynamic, 1> VectorXs; ///< variable size vector of real double type
-typedef Matrix<double, 1, 2> RowVector2s; ///< 2-row-vector of real double type
-typedef Matrix<double, 1, 3> RowVector3s; ///< 3-row-vector of real double type
-typedef Matrix<double, 1, 4> RowVector4s; ///< 4-row-vector of real double type
-typedef Matrix<double, 1, 7> RowVector7s; ///< 7-row-vector of real double type
-typedef Matrix<double, 1, Dynamic> RowVectorXs; ///< variable size row-vector of real double type
-
-// 2. Quaternions and other rotation things
-typedef Quaternion<double> Quaternions; ///< Quaternion of real double type
-typedef AngleAxis<double> AngleAxiss; ///< Angle-Axis of real double type
-typedef Rotation2D<double> Rotation2Ds; ///< Rotation2D of real double type
-typedef RotationBase<double, 3> Rotation3Ds; ///< Rotation3D of real double type
-
-// 3. Translations
-typedef Translation<double, 2> Translation2ds;
-typedef Translation<double, 3> Translation3ds;
-
-// 4. Transformations
-typedef Transform<double,2,Affine> Affine2ds; ///< Affine2d of real double type
-typedef Transform<double,3,Affine> Affine3ds; ///< Affine3d of real double type
-
-typedef Transform<double,2,Isometry> Isometry2ds; ///< Isometry2d of real double type
-typedef Transform<double,3,Isometry> Isometry3ds; ///< Isometry3d of real double type
-
-// 3. Sparse matrix
-typedef SparseMatrix<double, RowMajor, int> SparseMatrixs;
+typedef Matrix<double, 1, 1, RowMajor> Matrix1d; ///< 2x2 matrix of real double type
+typedef Matrix<double, 6, 6, RowMajor> Matrix6d; ///< 6x6 matrix of real double type
+//typedef Matrix<double, 7, 7, RowMajor> Matrix7d; ///< 7x7 matrix of real double type
+typedef Matrix<double, 1, 1> Vector1d; ///< 1-vector of real double type
+//typedef Matrix<double, 5, 1> Vector5d; ///< 5-vector of real double type
+typedef Matrix<double, 6, 1> Vector6d; ///< 6-vector of real double type
+typedef Matrix<double, 7, 1> Vector7d; ///< 7-vector of real double type
+//typedef Matrix<double, 8, 1> Vector8d; ///< 8-vector of real double type
+//typedef Matrix<double, 9, 1> Vector9d; ///< 9-vector of real double type
+//typedef Matrix<double, 10, 1> Vector10d; ///< 10-vector of real double type
+
+// 2. Sparse matrix
+typedef SparseMatrix<double, RowMajor, int> SparseMatrixd;
}
namespace wolf {
@@ -134,7 +104,7 @@ namespace wolf {
*
* Help:
*
- * The WOLF project implements many template functions using Eigen Matrix and Quaternions, in different versions
+ * The WOLF project implements many template functions using Eigen Matrix and Quaterniond, in different versions
* (Static size, Dynamic size, Map, Matrix expression).
*
* Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's StaticAssert.h):
@@ -295,8 +265,8 @@ inline bool file_exists(const std::string& _name)
return (stat (_name.c_str(), &buffer) == 0);
}
-inline const Eigen::Vector3s gravity(void) {
- return Eigen::Vector3s(0,0,-9.806);
+inline const Eigen::Vector3d gravity(void) {
+ return Eigen::Vector3d(0,0,-9.806);
}
template <typename T, int N, int RC>
diff --git a/include/core/factor/factor_analytic.h b/include/core/factor/factor_analytic.h
index 5ff81d8cd6e57b2e94a7288bd73bef4a6503cd36..71be266229ab2c01320375ae5269843c5978aa54 100644
--- a/include/core/factor/factor_analytic.h
+++ b/include/core/factor/factor_analytic.h
@@ -84,27 +84,27 @@ class FactorAnalytic: public FactorBase
virtual bool evaluate(double const* const* parameters, double* residuals, double** jacobians) const override
{
// load parameters evaluation value
- std::vector<Eigen::Map<const Eigen::VectorXs>> state_blocks_map_;
+ std::vector<Eigen::Map<const Eigen::VectorXd>> state_blocks_map_;
for (unsigned int i = 0; i < state_block_sizes_vector_.size(); i++)
- state_blocks_map_.push_back(Eigen::Map<const Eigen::VectorXs>((double*)parameters[i], state_block_sizes_vector_[i]));
+ state_blocks_map_.push_back(Eigen::Map<const Eigen::VectorXd>((double*)parameters[i], state_block_sizes_vector_[i]));
// residuals
- Eigen::Map<Eigen::VectorXs> residuals_map((double*)residuals, getSize());
+ Eigen::Map<Eigen::VectorXd> residuals_map((double*)residuals, getSize());
residuals_map = evaluateResiduals(state_blocks_map_);
// also compute jacobians
if (jacobians != nullptr)
{
- std::vector<Eigen::Map<Eigen::MatrixXs>> jacobians_map_;
+ std::vector<Eigen::Map<Eigen::MatrixXd>> jacobians_map_;
std::vector<bool> compute_jacobians_(state_block_sizes_vector_.size());
for (unsigned int i = 0; i < state_block_sizes_vector_.size(); i++)
{
compute_jacobians_[i] = (jacobians[i] != nullptr);
if (jacobians[i] != nullptr)
- jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>((double*)jacobians[i], getSize(), state_block_sizes_vector_[i]));
+ jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXd>((double*)jacobians[i], getSize(), state_block_sizes_vector_[i]));
else
- jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXs>(nullptr, 0, 0)); //TODO: check if it can be done
+ jacobians_map_.push_back(Eigen::Map<Eigen::MatrixXd>(nullptr, 0, 0)); //TODO: check if it can be done
}
// evaluate jacobians
@@ -118,20 +118,20 @@ class FactorAnalytic: public FactorBase
/** Returns the residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
**/
// TODO
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const override
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override
{
};
/** \brief Returns the residual evaluated in the states provided
*
- * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXs
+ * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
*
**/
- virtual Eigen::VectorXs evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector) const = 0;
+ virtual Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector) const = 0;
/** \brief Returns the normalized jacobians evaluated in the states
*
- * Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
+ * Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
@@ -139,7 +139,7 @@ class FactorAnalytic: public FactorBase
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
- virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector, std::vector<Eigen::Map<Eigen::MatrixXs>>& jacobians, const std::vector<bool>& _compute_jacobian) const = 0;
+ virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector, std::vector<Eigen::Map<Eigen::MatrixXd>>& jacobians, const std::vector<bool>& _compute_jacobian) const = 0;
/** \brief Returns the jacobians (without measurement noise normalization) evaluated in the current state blocks values
*
@@ -148,7 +148,7 @@ class FactorAnalytic: public FactorBase
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
*
**/
- virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const = 0;
+ virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const = 0;
/** \brief Returns the jacobians computation method
*
diff --git a/include/core/factor/factor_autodiff.h b/include/core/factor/factor_autodiff.h
index f574f26cecb6a6482f257687f9bed03802cf2a85..e5dd155944a7c691296a2705152e3ce5d903b8e3 100644
--- a/include/core/factor/factor_autodiff.h
+++ b/include/core/factor/factor_autodiff.h
@@ -223,7 +223,7 @@ class FactorAutodiff : public FactorBase
/** \brief Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
*
**/
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
{
assert(residual_.size() == RES);
jacobians_.clear();
@@ -233,7 +233,7 @@ class FactorAutodiff : public FactorBase
// init jacobian
for(unsigned int i = 0; i < n_blocks; ++i)
{
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ Eigen::MatrixXd Ji = Eigen::MatrixXd::Zero(RES, state_block_sizes_[i]);
jacobians_.push_back(Ji);
}
@@ -489,7 +489,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
(*jets_8_)[i].a = parameters[8][i];
}
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
{
assert(residual_.size() == RES);
jacobians_.clear();
@@ -499,7 +499,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,B8,0> : public FactorBase
// init jacobian
for(unsigned int i = 0; i < n_blocks; ++i)
{
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ Eigen::MatrixXd Ji = Eigen::MatrixXd::Zero(RES, state_block_sizes_[i]);
jacobians_.push_back(Ji);
}
@@ -731,7 +731,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
(*jets_7_)[i].a = parameters[7][i];
}
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
{
assert(residual_.size() == RES);
jacobians_.clear();
@@ -741,7 +741,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,B7,0,0> : public FactorBase
// init jacobian
for(unsigned int i = 0; i < n_blocks; ++i)
{
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ Eigen::MatrixXd Ji = Eigen::MatrixXd::Zero(RES, state_block_sizes_[i]);
jacobians_.push_back(Ji);
}
@@ -962,7 +962,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
(*jets_6_)[i].a = parameters[6][i];
}
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
{
assert(residual_.size() == RES);
jacobians_.clear();
@@ -972,7 +972,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,B6,0,0,0> : public FactorBase
// init jacobian
for(unsigned int i = 0; i < n_blocks; ++i)
{
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ Eigen::MatrixXd Ji = Eigen::MatrixXd::Zero(RES, state_block_sizes_[i]);
jacobians_.push_back(Ji);
}
@@ -1182,7 +1182,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
(*jets_5_)[i].a = parameters[5][i];
}
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
{
assert(residual_.size() == RES);
jacobians_.clear();
@@ -1192,7 +1192,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,B5,0,0,0,0> : public FactorBase
// init jacobian
for(unsigned int i = 0; i < n_blocks; ++i)
{
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ Eigen::MatrixXd Ji = Eigen::MatrixXd::Zero(RES, state_block_sizes_[i]);
jacobians_.push_back(Ji);
}
@@ -1386,7 +1386,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
(*jets_4_)[i].a = parameters[4][i];
}
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
{
assert(residual_.size() == RES);
jacobians_.clear();
@@ -1396,7 +1396,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,B4,0,0,0,0,0> : public FactorBase
// init jacobian
for(unsigned int i = 0; i < n_blocks; ++i)
{
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ Eigen::MatrixXd Ji = Eigen::MatrixXd::Zero(RES, state_block_sizes_[i]);
jacobians_.push_back(Ji);
}
@@ -1579,7 +1579,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
(*jets_3_)[i].a = parameters[3][i];
}
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
{
assert(residual_.size() == RES);
jacobians_.clear();
@@ -1589,7 +1589,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,B3,0,0,0,0,0,0> : public FactorBase
// init jacobian
for(unsigned int i = 0; i < n_blocks; ++i)
{
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ Eigen::MatrixXd Ji = Eigen::MatrixXd::Zero(RES, state_block_sizes_[i]);
jacobians_.push_back(Ji);
}
@@ -1765,7 +1765,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
(*jets_2_)[i].a = parameters[2][i];
}
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
{
assert(residual_.size() == RES);
jacobians_.clear();
@@ -1775,7 +1775,7 @@ class FactorAutodiff<FacT,RES,B0,B1,B2,0,0,0,0,0,0,0> : public FactorBase
// init jacobian
for(unsigned int i = 0; i < n_blocks; ++i)
{
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ Eigen::MatrixXd Ji = Eigen::MatrixXd::Zero(RES, state_block_sizes_[i]);
jacobians_.push_back(Ji);
}
@@ -1940,7 +1940,7 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
(*jets_1_)[i].a = parameters[1][i];
}
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
{
assert(residual_.size() == RES);
jacobians_.clear();
@@ -1950,7 +1950,7 @@ class FactorAutodiff<FacT,RES,B0,B1,0,0,0,0,0,0,0,0> : public FactorBase
// init jacobian
for(unsigned int i = 0; i < n_blocks; ++i)
{
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ Eigen::MatrixXd Ji = Eigen::MatrixXd::Zero(RES, state_block_sizes_[i]);
jacobians_.push_back(Ji);
}
@@ -2104,7 +2104,7 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
(*jets_0_)[i].a = parameters[0][i];
}
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const
{
assert(residual_.size() == RES);
jacobians_.clear();
@@ -2114,7 +2114,7 @@ class FactorAutodiff<FacT,RES,B0,0,0,0,0,0,0,0,0,0> : public FactorBase
// init jacobian
for(unsigned int i = 0; i < n_blocks; ++i)
{
- Eigen::MatrixXs Ji = Eigen::MatrixXs::Zero(RES, state_block_sizes_[i]);
+ Eigen::MatrixXd Ji = Eigen::MatrixXd::Zero(RES, state_block_sizes_[i]);
jacobians_.push_back(Ji);
}
diff --git a/include/core/factor/factor_base.h b/include/core/factor/factor_base.h
index 2252240311ca043749c4dfa4e1f6551176c96014..58ad77ae08d78efe9a0b5454551a3d69dfd21d50 100644
--- a/include/core/factor/factor_base.h
+++ b/include/core/factor/factor_base.h
@@ -90,7 +90,7 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** Returns a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr and the residual vector
**/
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXs& _residual, std::vector<Eigen::MatrixXs>& _jacobians) const = 0;
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& _residual, std::vector<Eigen::MatrixXd>& _jacobians) const = 0;
/** \brief Returns the jacobians computation method
**/
@@ -106,15 +106,15 @@ class FactorBase : public NodeBase, public std::enable_shared_from_this<FactorBa
/** \brief Returns a reference to the feature measurement
**/
- virtual const Eigen::VectorXs& getMeasurement() const;
+ virtual const Eigen::VectorXd& getMeasurement() const;
/** \brief Returns a reference to the feature measurement covariance
**/
- virtual const Eigen::MatrixXs& getMeasurementCovariance() const;
+ virtual const Eigen::MatrixXd& getMeasurementCovariance() const;
/** \brief Returns a reference to the feature measurement square root information
**/
- virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const;
+ virtual const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const;
/** \brief Returns a pointer to the feature constrained from
**/
diff --git a/include/core/factor/factor_block_absolute.h b/include/core/factor/factor_block_absolute.h
index 2eed31556681df9d1c6f07150627c8689870c144..43275c6241a9d26e78a04096a4e886cfcf179158 100644
--- a/include/core/factor/factor_block_absolute.h
+++ b/include/core/factor/factor_block_absolute.h
@@ -24,7 +24,7 @@ class FactorBlockAbsolute : public FactorAnalytic
SizeEigen sb_size_; // the size of the state block
SizeEigen sb_constrained_start_; // the index of the first state element that is constrained
SizeEigen sb_constrained_size_; // the size of the state segment that is constrained
- Eigen::MatrixXs J_; // Jacobian
+ Eigen::MatrixXd J_; // Jacobian
public:
@@ -52,11 +52,11 @@ class FactorBlockAbsolute : public FactorAnalytic
// precompute Jacobian (Identity)
if (sb_constrained_start_ == 0)
- J_ = Eigen::MatrixXs::Identity(sb_constrained_size_, sb_size_);
+ J_ = Eigen::MatrixXd::Identity(sb_constrained_size_, sb_size_);
else
{
- J_ = Eigen::MatrixXs::Zero(sb_constrained_size_,sb_size_);
- J_.middleCols(sb_constrained_start_,sb_constrained_size_) = Eigen::MatrixXs::Identity(sb_constrained_size_, sb_constrained_size_);
+ J_ = Eigen::MatrixXd::Zero(sb_constrained_size_,sb_size_);
+ J_.middleCols(sb_constrained_start_,sb_constrained_size_) = Eigen::MatrixXd::Identity(sb_constrained_size_, sb_constrained_size_);
}
}
@@ -64,14 +64,14 @@ class FactorBlockAbsolute : public FactorAnalytic
/** \brief Returns the residual evaluated in the states provided
*
- * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXs
+ * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
*
**/
- virtual Eigen::VectorXs evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const;
+ virtual Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const;
/** \brief Returns the normalized jacobians evaluated in the states
*
- * Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
+ * Returns the normalized (by the measurement noise sqrt information) jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
@@ -79,8 +79,8 @@ class FactorBlockAbsolute : public FactorAnalytic
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
- virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector,
- std::vector<Eigen::Map<Eigen::MatrixXs> >& jacobians,
+ virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
@@ -90,14 +90,14 @@ class FactorBlockAbsolute : public FactorAnalytic
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
*
**/
- virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const;
+ virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const;
/** \brief Returns the factor residual size
**/
virtual unsigned int getSize() const;
};
-inline Eigen::VectorXs FactorBlockAbsolute::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const
+inline Eigen::VectorXd FactorBlockAbsolute::evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
{
assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
assert(_st_vector.front().size() >= getMeasurement().size() && "Wrong StateBlock size");
@@ -110,8 +110,8 @@ inline Eigen::VectorXs FactorBlockAbsolute::evaluateResiduals(const std::vector<
}
inline void FactorBlockAbsolute::evaluateJacobians(
- const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector,
- std::vector<Eigen::Map<Eigen::MatrixXs> >& jacobians, const std::vector<bool>& _compute_jacobian) const
+ const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixXd> >& jacobians, const std::vector<bool>& _compute_jacobian) const
{
assert(_st_vector.size() == 1 && "Wrong number of state blocks!");
assert(jacobians.size() == 1 && "Wrong number of jacobians!");
@@ -124,7 +124,7 @@ inline void FactorBlockAbsolute::evaluateJacobians(
jacobians.front() = getMeasurementSquareRootInformationUpper() * J_;
}
-inline void FactorBlockAbsolute::evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const
+inline void FactorBlockAbsolute::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
{
assert(jacobians.size() == 1 && "Wrong number of jacobians!");
diff --git a/include/core/factor/factor_diff_drive.h b/include/core/factor/factor_diff_drive.h
index 274c226ed779ced623f5ae8c0eb76c0e5bc4d291..c52c3a7cdb528be6f5892c5854aef85ba2e1c12d 100644
--- a/include/core/factor/factor_diff_drive.h
+++ b/include/core/factor/factor_diff_drive.h
@@ -84,12 +84,12 @@ class FactorDiffDrive : public FactorAutodiff<FactorDiffDrive,
bool operator ()(const T* const _p1, const T* const _o1, const T* const _p2,
const T* const _o2, const T* const _c, T* _residuals) const;
- VectorXs residual();
+ VectorXd residual();
protected:
- Eigen::Vector3s calib_preint_;
- Eigen::MatrixXs J_delta_calib_;
+ Eigen::Vector3d calib_preint_;
+ Eigen::MatrixXd J_delta_calib_;
};
template<typename T>
@@ -138,9 +138,9 @@ inline bool FactorDiffDrive::operator ()(const T* const _p1, const T* const _o1,
return true;
}
-inline Eigen::VectorXs FactorDiffDrive::residual()
+inline Eigen::VectorXd FactorDiffDrive::residual()
{
- VectorXs residual(3);
+ VectorXd residual(3);
operator ()(getFrameOther()->getP()->getState().data(), getFrameOther()->getO()->getState().data(),
getCapture()->getFrame()->getP()->getState().data(),
getCapture()->getFrame()->getO()->getState().data(),
diff --git a/include/core/factor/factor_odom_2D.h b/include/core/factor/factor_odom_2D.h
index a580b1d087e6d5095f23000d739ac4598fcff2cc..4cd1628723beffc7f7f4e5bb1ad5a0a02d0a00eb 100644
--- a/include/core/factor/factor_odom_2D.h
+++ b/include/core/factor/factor_odom_2D.h
@@ -76,7 +76,7 @@ inline bool FactorOdom2D::operator ()(const T* const _p1, const T* const _o1, co
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
// using ceres::Jet;
-// Eigen::MatrixXs J(3,6);
+// Eigen::MatrixXd J(3,6);
// J.row(0) = ((Jet<double, 6>)(er(0))).v;
// J.row(1) = ((Jet<double, 6>)(er(1))).v;
// J.row(2) = ((Jet<double, 6>)(er(2))).v;
diff --git a/include/core/factor/factor_odom_2D_analytic.h b/include/core/factor/factor_odom_2D_analytic.h
index d4b77865120cd7480dcc1eb80566b2116b50d73a..52f3ff1bf70173c2a8d75cdf2a25cd72b90f0c55 100644
--- a/include/core/factor/factor_odom_2D_analytic.h
+++ b/include/core/factor/factor_odom_2D_analytic.h
@@ -38,16 +38,16 @@ class FactorOdom2DAnalytic : public FactorRelative2DAnalytic
//
// /** \brief Returns the residual evaluated in the states provided
// *
-// * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXs
+// * Returns the residual evaluated in the states provided in std::vector of mapped Eigen::VectorXd
// *
// **/
-// virtual Eigen::VectorXs evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector) const
+// virtual Eigen::VectorXd evaluateResiduals(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector) const
// {
-// Eigen::VectorXs residual(3);
-// Eigen::VectorXs expected_measurement(3);
+// Eigen::VectorXd residual(3);
+// Eigen::VectorXd expected_measurement(3);
//
// // Expected measurement
-// Eigen::Matrix2s R = Eigen::Rotation2D<double>(-_st_vector[1](0)).matrix();
+// Eigen::Matrix2d R = Eigen::Rotation2D<double>(-_st_vector[1](0)).matrix();
// expected_measurement.head(2) = R * (_st_vector[2]-_st_vector[0]); // rotar menys l'angle de primer (-_o1)
// expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0);
//
@@ -64,7 +64,7 @@ class FactorOdom2DAnalytic : public FactorRelative2DAnalytic
//
// /** \brief Returns the jacobians evaluated in the states provided
// *
-// * Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
+// * Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
// * IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
// *
// * \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
@@ -72,7 +72,7 @@ class FactorOdom2DAnalytic : public FactorRelative2DAnalytic
// * \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
// *
// **/
-// virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXs>>& _st_vector, std::vector<Eigen::Map<Eigen::MatrixXs>>& jacobians, const std::vector<bool>& _compute_jacobian) const
+// virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd>>& _st_vector, std::vector<Eigen::Map<Eigen::MatrixXd>>& jacobians, const std::vector<bool>& _compute_jacobian) const
// {
// jacobians[0] << -cos(_st_vector[1](0)), -sin(_st_vector[1](0)),
// sin(_st_vector[1](0)), -cos(_st_vector[1](0)),
diff --git a/include/core/factor/factor_odom_3D.h b/include/core/factor/factor_odom_3D.h
index 49d5365609980fc66aee82d516e7ab4cb5fd9b78..cfcdf95e93b960da9d5d4aa3eda6cbba59f3e227 100644
--- a/include/core/factor/factor_odom_3D.h
+++ b/include/core/factor/factor_odom_3D.h
@@ -43,7 +43,7 @@ class FactorOdom3D : public FactorAutodiff<FactorOdom3D,6,3,4,3,4>
T* _expectation_dp,
T* _expectation_dq) const;
- Eigen::VectorXs expectation() const;
+ Eigen::VectorXd expectation() const;
private:
template<typename T>
@@ -139,15 +139,15 @@ inline bool FactorOdom3D::expectation(const T* const _p_current, const T* const
return true;
}
-inline Eigen::VectorXs FactorOdom3D::expectation() const
+inline Eigen::VectorXd FactorOdom3D::expectation() const
{
- Eigen::VectorXs exp(7);
+ Eigen::VectorXd exp(7);
FrameBasePtr frm_current = getFeature()->getFrame();
FrameBasePtr frm_past = getFrameOther();
- const Eigen::VectorXs frame_current_pos = frm_current->getP()->getState();
- const Eigen::VectorXs frame_current_ori = frm_current->getO()->getState();
- const Eigen::VectorXs frame_past_pos = frm_past->getP()->getState();
- const Eigen::VectorXs frame_past_ori = frm_past->getO()->getState();
+ const Eigen::VectorXd frame_current_pos = frm_current->getP()->getState();
+ const Eigen::VectorXd frame_current_ori = frm_current->getO()->getState();
+ const Eigen::VectorXd frame_past_pos = frm_past->getP()->getState();
+ const Eigen::VectorXd frame_past_ori = frm_past->getO()->getState();
// std::cout << "frame_current_pos: " << frm_current->getP()->getState().transpose() << std::endl;
// std::cout << "frame_past_pos: " << frm_past->getP()->getState().transpose() << std::endl;
diff --git a/include/core/factor/factor_pose_2D.h b/include/core/factor/factor_pose_2D.h
index f6541cc1cea62a24d7a84b3bf309023885612492..5317f4131e1d4b5cc01151e867368a7e57227180 100644
--- a/include/core/factor/factor_pose_2D.h
+++ b/include/core/factor/factor_pose_2D.h
@@ -53,7 +53,7 @@ inline bool FactorPose2D::operator ()(const T* const _p, const T* const _o, T* _
////////////////////////////////////////////////////////
// print Jacobian. Uncomment this as you wish (remember to uncomment #include "ceres/jet.h" above):
// using ceres::Jet;
-// Eigen::MatrixXs J(3,3);
+// Eigen::MatrixXd J(3,3);
// J.row(0) = ((Jet<double, 3>)(er(0))).v;
// J.row(1) = ((Jet<double, 3>)(er(1))).v;
// J.row(2) = ((Jet<double, 3>)(er(2))).v;
diff --git a/include/core/factor/factor_pose_3D.h b/include/core/factor/factor_pose_3D.h
index d36bcdb32a832695b29bbbe5a0610ded37746218..da27b6e2ab6ae6c3edbaf827460f95fe6849affb 100644
--- a/include/core/factor/factor_pose_3D.h
+++ b/include/core/factor/factor_pose_3D.h
@@ -38,8 +38,8 @@ inline bool FactorPose3D::operator ()(const T* const _p, const T* const _o, T* _
Eigen::Quaternion<T> q(_o);
// measurements
- Eigen::Vector3s p_measured(getMeasurement().data() + 0);
- Eigen::Quaternions q_measured(getMeasurement().data() + 3);
+ Eigen::Vector3d p_measured(getMeasurement().data() + 0);
+ Eigen::Quaterniond q_measured(getMeasurement().data() + 3);
// error
Eigen::Matrix<T, 6, 1> er;
diff --git a/include/core/factor/factor_quaternion_absolute.h b/include/core/factor/factor_quaternion_absolute.h
index 477db6368bb4217b41e154095cbca3bd2c00cf6f..52d6809a6036e407f130766d3076de0fb6c18d41 100644
--- a/include/core/factor/factor_quaternion_absolute.h
+++ b/include/core/factor/factor_quaternion_absolute.h
@@ -45,7 +45,7 @@ inline bool FactorQuaternionAbsolute::operator ()(const T* const _o, T* _residua
Eigen::Quaternion<T> q1(_o);
// measurements
- Eigen::Quaternions q2(getMeasurement().data() + 0); //q_measured
+ Eigen::Quaterniond q2(getMeasurement().data() + 0); //q_measured
/* error
* to compute the difference between both quaternions, we do
diff --git a/include/core/factor/factor_relative_2D_analytic.h b/include/core/factor/factor_relative_2D_analytic.h
index 43b376b5d576955b25d5ac7bc064d0c45224aa02..84357a52787e1140065d23b46c3b0b427bcbd30b 100644
--- a/include/core/factor/factor_relative_2D_analytic.h
+++ b/include/core/factor/factor_relative_2D_analytic.h
@@ -65,14 +65,14 @@ class FactorRelative2DAnalytic : public FactorAnalytic
/** \brief Returns the residual evaluated in the states provided
*
- * Returns the residual evaluated in the states provided in a std::vector of mapped Eigen::VectorXs
+ * Returns the residual evaluated in the states provided in a std::vector of mapped Eigen::VectorXd
**/
- virtual Eigen::VectorXs evaluateResiduals(
- const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const override;
+ virtual Eigen::VectorXd evaluateResiduals(
+ const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const override;
/** \brief Returns the jacobians evaluated in the states provided
*
- * Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXs.
+ * Returns the jacobians evaluated in the states provided in std::vector of mapped Eigen::VectorXd.
* IMPORTANT: only fill the jacobians of the state blocks specified in _compute_jacobian.
*
* \param _st_vector is a vector containing the mapped eigen vectors of all state blocks involved in the factor
@@ -80,27 +80,27 @@ class FactorRelative2DAnalytic : public FactorAnalytic
* \param _compute_jacobian is a vector that specifies whether the ith jacobian sould be computed or not
*
**/
- virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector,
- std::vector<Eigen::Map<Eigen::MatrixXs> >& jacobians,
+ virtual void evaluateJacobians(const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixXd> >& jacobians,
const std::vector<bool>& _compute_jacobian) const override;
/** \brief Returns the pure jacobians (without measurement noise) evaluated in the state blocks values
* \param jacobians is an output vector of mapped eigen matrices that sould contain the jacobians w.r.t each state block
*
**/
- virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const override;
+ virtual void evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const override;
};
/// IMPLEMENTATION ///
-inline Eigen::VectorXs FactorRelative2DAnalytic::evaluateResiduals(
- const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector) const
+inline Eigen::VectorXd FactorRelative2DAnalytic::evaluateResiduals(
+ const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector) const
{
- Eigen::VectorXs residual(3);
- Eigen::VectorXs expected_measurement(3);
+ Eigen::VectorXd residual(3);
+ Eigen::VectorXd expected_measurement(3);
// Expected measurement
- Eigen::Matrix2s R = Eigen::Rotation2D<double>(-_st_vector[1](0)).matrix();
+ Eigen::Matrix2d R = Eigen::Rotation2D<double>(-_st_vector[1](0)).matrix();
expected_measurement.head(2) = R * (_st_vector[2] - _st_vector[0]); // rotar menys l'angle de primer (-_o1)
expected_measurement(2) = _st_vector[3](0) - _st_vector[1](0);
// Residual
@@ -114,8 +114,8 @@ inline Eigen::VectorXs FactorRelative2DAnalytic::evaluateResiduals(
}
inline void FactorRelative2DAnalytic::evaluateJacobians(
- const std::vector<Eigen::Map<const Eigen::VectorXs> >& _st_vector,
- std::vector<Eigen::Map<Eigen::MatrixXs> >& jacobians, const std::vector<bool>& _compute_jacobian) const
+ const std::vector<Eigen::Map<const Eigen::VectorXd> >& _st_vector,
+ std::vector<Eigen::Map<Eigen::MatrixXd> >& jacobians, const std::vector<bool>& _compute_jacobian) const
{
jacobians[0] << -cos(_st_vector[1](0)), -sin(_st_vector[1](0)), sin(_st_vector[1](0)), -cos(_st_vector[1](0)), 0, 0;
jacobians[0] = getMeasurementSquareRootInformationUpper() * jacobians[0];
@@ -130,7 +130,7 @@ inline void FactorRelative2DAnalytic::evaluateJacobians(
jacobians[3] = getMeasurementSquareRootInformationUpper() * jacobians[3];
}
-inline void FactorRelative2DAnalytic::evaluatePureJacobians(std::vector<Eigen::MatrixXs>& jacobians) const
+inline void FactorRelative2DAnalytic::evaluatePureJacobians(std::vector<Eigen::MatrixXd>& jacobians) const
{
jacobians[0]
<< -cos(getStateBlockPtrVector()[1]->getState()(0)), -sin(getStateBlockPtrVector()[1]->getState()(0)), sin(
diff --git a/include/core/feature/feature_base.h b/include/core/feature/feature_base.h
index cfa5810328970be6cf201cd37a72d85b6827ba23..c008b58b97ccd6e4596341cba1f56f034f23ca53 100644
--- a/include/core/feature/feature_base.h
+++ b/include/core/feature/feature_base.h
@@ -33,10 +33,10 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
unsigned int feature_id_;
unsigned int track_id_; // ID of the feature track
unsigned int landmark_id_; // ID of the landmark
- Eigen::VectorXs measurement_; ///< the measurement vector
- Eigen::MatrixXs measurement_covariance_; ///< the measurement covariance matrix
- Eigen::MatrixXs measurement_sqrt_information_upper_; ///< the squared root information matrix
- Eigen::VectorXs expectation_; ///< expectation
+ Eigen::VectorXd measurement_; ///< the measurement vector
+ Eigen::MatrixXd measurement_covariance_; ///< the measurement covariance matrix
+ Eigen::MatrixXd measurement_sqrt_information_upper_; ///< the squared root information matrix
+ Eigen::VectorXd expectation_; ///< expectation
virtual void setProblem(ProblemPtr _problem) final;
public:
@@ -53,7 +53,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
* \param _measurement the measurement
* \param _meas_covariance the noise of the measurement
*/
- FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_uncertainty, UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
+ FeatureBase(const std::string& _type, const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_uncertainty, UncertaintyType _uncertainty_type = UNCERTAINTY_IS_COVARIANCE);
virtual ~FeatureBase();
virtual void remove(bool viral_remove_empty_parent=false);
@@ -72,16 +72,16 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
* WARNING: To be fast, it does not check that index _ii is smaller than dimension.
*/
double getMeasurement(unsigned int _ii) const;
- const Eigen::VectorXs& getMeasurement() const;
- void setMeasurement(const Eigen::VectorXs& _meas);
- void setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov);
- void setMeasurementInformation(const Eigen::MatrixXs & _meas_info);
- const Eigen::MatrixXs& getMeasurementCovariance() const;
- Eigen::MatrixXs getMeasurementInformation() const;
- const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const;
+ const Eigen::VectorXd& getMeasurement() const;
+ void setMeasurement(const Eigen::VectorXd& _meas);
+ void setMeasurementCovariance(const Eigen::MatrixXd & _meas_cov);
+ void setMeasurementInformation(const Eigen::MatrixXd & _meas_info);
+ const Eigen::MatrixXd& getMeasurementCovariance() const;
+ Eigen::MatrixXd getMeasurementInformation() const;
+ const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const;
- const Eigen::VectorXs& getExpectation() const;
- void setExpectation(const Eigen::VectorXs& expectation);
+ const Eigen::VectorXd& getExpectation() const;
+ void setExpectation(const Eigen::VectorXd& expectation);
// wolf tree access
FrameBasePtr getFrame() const;
@@ -103,7 +103,7 @@ class FeatureBase : public NodeBase, public std::enable_shared_from_this<Feature
protected:
- Eigen::MatrixXs computeSqrtUpper(const Eigen::MatrixXs& _M) const;
+ Eigen::MatrixXd computeSqrtUpper(const Eigen::MatrixXd& _M) const;
private:
void setCapture(CaptureBasePtr _cap_ptr){capture_ptr_ = _cap_ptr;}
@@ -154,37 +154,37 @@ inline double FeatureBase::getMeasurement(unsigned int _ii) const
return measurement_(_ii);
}
-inline const Eigen::VectorXs& FeatureBase::getMeasurement() const
+inline const Eigen::VectorXd& FeatureBase::getMeasurement() const
{
return measurement_;
}
-inline const Eigen::MatrixXs& FeatureBase::getMeasurementCovariance() const
+inline const Eigen::MatrixXd& FeatureBase::getMeasurementCovariance() const
{
return measurement_covariance_;
}
-inline Eigen::MatrixXs FeatureBase::getMeasurementInformation() const
+inline Eigen::MatrixXd FeatureBase::getMeasurementInformation() const
{
return measurement_sqrt_information_upper_.transpose() * measurement_sqrt_information_upper_;
}
-inline const Eigen::MatrixXs& FeatureBase::getMeasurementSquareRootInformationUpper() const
+inline const Eigen::MatrixXd& FeatureBase::getMeasurementSquareRootInformationUpper() const
{
return measurement_sqrt_information_upper_;
}
-inline void FeatureBase::setMeasurement(const Eigen::VectorXs& _meas)
+inline void FeatureBase::setMeasurement(const Eigen::VectorXd& _meas)
{
measurement_ = _meas;
}
-inline const Eigen::VectorXs& FeatureBase::getExpectation() const
+inline const Eigen::VectorXd& FeatureBase::getExpectation() const
{
return expectation_;
}
-inline void FeatureBase::setExpectation(const Eigen::VectorXs& expectation)
+inline void FeatureBase::setExpectation(const Eigen::VectorXd& expectation)
{
expectation_ = expectation;
}
diff --git a/include/core/feature/feature_diff_drive.h b/include/core/feature/feature_diff_drive.h
index 1261db31dc17429733b0d3fc6bf09b96d3e8b11b..203e8f05facaf95ef18543f0ffe6508a69a70060 100644
--- a/include/core/feature/feature_diff_drive.h
+++ b/include/core/feature/feature_diff_drive.h
@@ -19,10 +19,10 @@ class FeatureDiffDrive : public FeatureMotion
{
public:
- FeatureDiffDrive(const Eigen::VectorXs& _delta_preintegrated,
- const Eigen::MatrixXs& _delta_preintegrated_covariance,
- const Eigen::VectorXs& _diff_drive_params,
- const Eigen::MatrixXs& _jacobian_diff_drive_params);
+ FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::VectorXd& _diff_drive_params,
+ const Eigen::MatrixXd& _jacobian_diff_drive_params);
virtual ~FeatureDiffDrive() = default;
diff --git a/include/core/feature/feature_motion.h b/include/core/feature/feature_motion.h
index 9f531f06103412cf34f053c8cb2bfef4fd9ba851..ae588e29a7a6ad8de83b601b87af7c41903d33c9 100644
--- a/include/core/feature/feature_motion.h
+++ b/include/core/feature/feature_motion.h
@@ -20,32 +20,32 @@ class FeatureMotion : public FeatureBase
{
public:
FeatureMotion(const std::string& _type,
- const VectorXs& _delta_preint,
- const MatrixXs _delta_preint_cov,
- const VectorXs& _calib_preint,
- const MatrixXs& _jacobian_calib);
+ const VectorXd& _delta_preint,
+ const MatrixXd _delta_preint_cov,
+ const VectorXd& _calib_preint,
+ const MatrixXd& _jacobian_calib);
virtual ~FeatureMotion();
- const Eigen::VectorXs& getDeltaPreint() const; ///< A new name for getMeasurement()
- const Eigen::VectorXs& getCalibrationPreint() const;
- const Eigen::MatrixXs& getJacobianCalibration() const;
+ const Eigen::VectorXd& getDeltaPreint() const; ///< A new name for getMeasurement()
+ const Eigen::VectorXd& getCalibrationPreint() const;
+ const Eigen::MatrixXd& getJacobianCalibration() const;
private:
- Eigen::VectorXs calib_preint_;
- Eigen::MatrixXs jacobian_calib_;
+ Eigen::VectorXd calib_preint_;
+ Eigen::MatrixXd jacobian_calib_;
};
-inline const Eigen::VectorXs& FeatureMotion::getDeltaPreint() const
+inline const Eigen::VectorXd& FeatureMotion::getDeltaPreint() const
{
return measurement_;
}
-inline const Eigen::VectorXs& FeatureMotion::getCalibrationPreint() const
+inline const Eigen::VectorXd& FeatureMotion::getCalibrationPreint() const
{
return calib_preint_;
}
-inline const Eigen::MatrixXs& FeatureMotion::getJacobianCalibration() const
+inline const Eigen::MatrixXd& FeatureMotion::getJacobianCalibration() const
{
return jacobian_calib_;
}
diff --git a/include/core/feature/feature_odom_2D.h b/include/core/feature/feature_odom_2D.h
index 5df94a665faecac2d17710df0f0b4d3496eb0a4d..3a50759802870400e19b3851959297295cc2aa55 100644
--- a/include/core/feature/feature_odom_2D.h
+++ b/include/core/feature/feature_odom_2D.h
@@ -23,7 +23,7 @@ class FeatureOdom2D : public FeatureBase
* \param _meas_covariance the noise of the measurement
*
*/
- FeatureOdom2D(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
+ FeatureOdom2D(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
virtual ~FeatureOdom2D();
diff --git a/include/core/feature/feature_pose.h b/include/core/feature/feature_pose.h
index 3fe413f6ad43d52c13463a11b53adb0bfcede846..2da7d5b8f367323d756a9dc32d080e25c5e9d254 100644
--- a/include/core/feature/feature_pose.h
+++ b/include/core/feature/feature_pose.h
@@ -22,7 +22,7 @@ class FeaturePose : public FeatureBase
* \param _meas_covariance the noise of the measurement
*
*/
- FeaturePose(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
+ FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance);
virtual ~FeaturePose();
};
diff --git a/include/core/frame/frame_base.h b/include/core/frame/frame_base.h
index aed6ff774ed52c5d0d4023188515823feca7ffab..32d082b3c1eea17f95573e3fdf724d3d135a7feb 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -68,7 +68,7 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
**/
FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr _p_ptr, StateBlockPtr _o_ptr = nullptr, StateBlockPtr _v_ptr = nullptr);
- FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x);
+ FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x);
virtual ~FrameBase();
virtual void remove(bool viral_remove_empty_parent=false);
@@ -122,11 +122,11 @@ class FrameBase : public NodeBase, public std::enable_shared_from_this<FrameBase
// States
public:
- void setState(const Eigen::VectorXs& _state);
- Eigen::VectorXs getState() const;
- void getState(Eigen::VectorXs& _state) const;
+ void setState(const Eigen::VectorXd& _state);
+ Eigen::VectorXd getState() const;
+ void getState(Eigen::VectorXd& _state) const;
unsigned int getSize() const;
- bool getCovariance(Eigen::MatrixXs& _cov) const;
+ bool getCovariance(Eigen::MatrixXd& _cov) const;
// Wolf tree access ---------------------------------------------------
public:
diff --git a/include/core/landmark/landmark_base.h b/include/core/landmark/landmark_base.h
index 0737daf2bd47b1d7ac70ec2cb615a5dc4e678882..37e20a86d86dad1a6483c97b84be3b24c0dc6ed9 100644
--- a/include/core/landmark/landmark_base.h
+++ b/include/core/landmark/landmark_base.h
@@ -36,7 +36,7 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
virtual void setProblem(ProblemPtr _problem) final;
unsigned int landmark_id_; ///< landmark unique id
TimeStamp stamp_; ///< stamp of the creation of the landmark
- Eigen::VectorXs descriptor_; //TODO: agree? JS: No: It is not general enough as descriptor to be in LmkBase.
+ Eigen::VectorXd descriptor_; //TODO: agree? JS: No: It is not general enough as descriptor to be in LmkBase.
public:
@@ -72,9 +72,9 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
StateBlockPtr getO() const;
void setP(const StateBlockPtr _p_ptr);
void setO(const StateBlockPtr _o_ptr);
- Eigen::VectorXs getState() const;
- void getState(Eigen::VectorXs& _state) const;
- bool getCovariance(Eigen::MatrixXs& _cov) const;
+ Eigen::VectorXd getState() const;
+ void getState(Eigen::VectorXd& _state) const;
+ bool getCovariance(Eigen::MatrixXd& _cov) const;
protected:
virtual void registerNewStateBlocks();
@@ -82,9 +82,9 @@ class LandmarkBase : public NodeBase, public std::enable_shared_from_this<Landma
// Descriptor
public:
- const Eigen::VectorXs& getDescriptor() const;
+ const Eigen::VectorXd& getDescriptor() const;
double getDescriptor(unsigned int _ii) const;
- void setDescriptor(const Eigen::VectorXs& _descriptor);
+ void setDescriptor(const Eigen::VectorXd& _descriptor);
unsigned int getHits() const;
@@ -200,7 +200,7 @@ inline void LandmarkBase::setO(const StateBlockPtr _st_ptr)
setStateBlock(1, _st_ptr);
}
-inline void LandmarkBase::setDescriptor(const Eigen::VectorXs& _descriptor)
+inline void LandmarkBase::setDescriptor(const Eigen::VectorXd& _descriptor)
{
descriptor_ = _descriptor;
}
@@ -211,7 +211,7 @@ inline double LandmarkBase::getDescriptor(unsigned int _ii) const
return descriptor_(_ii);
}
-inline const Eigen::VectorXs& LandmarkBase::getDescriptor() const
+inline const Eigen::VectorXd& LandmarkBase::getDescriptor() const
{
return descriptor_;
}
diff --git a/include/core/math/SE2.h b/include/core/math/SE2.h
index a32254de578f636a696dd91148bbd6cabac2b34f..00f570faed8c0542a1e0312a5ad1bc7c2ab710e3 100644
--- a/include/core/math/SE2.h
+++ b/include/core/math/SE2.h
@@ -35,7 +35,7 @@ Eigen::Matrix<T, 2, 2> exp(const T theta)
namespace SE2{
template<typename T>
-Eigen::Matrix2s V_helper(const T theta)
+Eigen::Matrix2d V_helper(const T theta)
{
T s; // sin(theta) / theta
T c_1; // (1-cos(theta)) / theta
diff --git a/include/core/math/SE3.h b/include/core/math/SE3.h
index 12c6bd62f2d3109bb78ce29f0e85ae4d20374078..9acfc971027378db119d4c96e78d94793614b631 100644
--- a/include/core/math/SE3.h
+++ b/include/core/math/SE3.h
@@ -166,7 +166,7 @@ inline void compose(const MatrixBase<D1>& d1,
// Jac wrt second delta
J_sum_d2.setIdentity(); //
J_sum_d2.block(0,0,3,3) = dR1; // dDp'/ddp
- // J_sum_d2.block(3,3,3,3) = Matrix3s::Identity(); // dDo'/ddo = I
+ // J_sum_d2.block(3,3,3,3) = Matrix3d::Identity(); // dDo'/ddo = I
// compose deltas -- done here to avoid aliasing when calling with input `d1` and result `sum` referencing the same variable
compose(d1, d2, sum);
diff --git a/include/core/problem/problem.h b/include/core/problem/problem.h
index 814588227769cf4592d79390a86ecdeffeee7d6f..362235b95fed754822e35e8e7082258d5bc194a6 100644
--- a/include/core/problem/problem.h
+++ b/include/core/problem/problem.h
@@ -46,7 +46,7 @@ class Problem : public std::enable_shared_from_this<Problem>
TrajectoryBasePtr trajectory_ptr_;
MapBasePtr map_ptr_;
ProcessorMotionPtr processor_motion_ptr_;
- std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXs> covariances_;
+ std::map<std::pair<StateBlockPtr, StateBlockPtr>, Eigen::MatrixXd> covariances_;
SizeEigen state_size_, state_cov_size_, dim_;
std::map<FactorBasePtr, Notification> factor_notification_map_;
std::map<StateBlockPtr, Notification> state_block_notification_map_;
@@ -83,7 +83,7 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
SensorBasePtr installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
- const Eigen::VectorXs& _extrinsics, //
+ const Eigen::VectorXd& _extrinsics, //
IntrinsicsBasePtr _intrinsics = nullptr);
/** \brief Factory method to install (create and add) sensors only from its properties -- Helper method loading parameters from file
@@ -94,7 +94,7 @@ class Problem : public std::enable_shared_from_this<Problem>
*/
SensorBasePtr installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
- const Eigen::VectorXs& _extrinsics, //
+ const Eigen::VectorXd& _extrinsics, //
const std::string& _intrinsics_filename);
/**
Custom installSensor using the parameters server
@@ -156,8 +156,8 @@ class Problem : public std::enable_shared_from_this<Problem>
// Trajectory branch ----------------------------------
TrajectoryBasePtr getTrajectory() const;
- virtual FrameBasePtr setPrior(const Eigen::VectorXs& _prior_state, //
- const Eigen::MatrixXs& _prior_cov, //
+ virtual FrameBasePtr setPrior(const Eigen::VectorXd& _prior_state, //
+ const Eigen::MatrixXd& _prior_cov, //
const TimeStamp& _ts,
const double _time_tolerance);
@@ -176,7 +176,7 @@ class Problem : public std::enable_shared_from_this<Problem>
FrameBasePtr emplaceFrame(const std::string& _frame_structure, //
const SizeEigen _dim, //
FrameType _frame_key_type, //
- const Eigen::VectorXs& _frame_state, //
+ const Eigen::VectorXd& _frame_state, //
const TimeStamp& _time_stamp);
/** \brief Emplace frame from string frame_structure without state
@@ -206,7 +206,7 @@ class Problem : public std::enable_shared_from_this<Problem>
* - If it is key-frame, update state-block lists in Problem
*/
FrameBasePtr emplaceFrame(FrameType _frame_key_type, //
- const Eigen::VectorXs& _frame_state, //
+ const Eigen::VectorXd& _frame_state, //
const TimeStamp& _time_stamp);
/** \brief Emplace frame from string frame_structure without structure nor state
@@ -261,13 +261,13 @@ class Problem : public std::enable_shared_from_this<Problem>
const double& _time_tolerance);
// State getters
- Eigen::VectorXs getCurrentState ( ) const;
- void getCurrentState (Eigen::VectorXs& _state) const;
- void getCurrentStateAndStamp (Eigen::VectorXs& _state, TimeStamp& _ts) const;
- Eigen::VectorXs getState (const TimeStamp& _ts) const;
- void getState (const TimeStamp& _ts, Eigen::VectorXs& _state) const;
+ Eigen::VectorXd getCurrentState ( ) const;
+ void getCurrentState (Eigen::VectorXd& _state) const;
+ void getCurrentStateAndStamp (Eigen::VectorXd& _state, TimeStamp& _ts) const;
+ Eigen::VectorXd getState (const TimeStamp& _ts) const;
+ void getState (const TimeStamp& _ts, Eigen::VectorXd& _state) const;
// Zero state provider
- Eigen::VectorXs zeroState ( ) const;
+ Eigen::VectorXd zeroState ( ) const;
bool priorIsSet() const;
// Map branch -----------------------------------------
@@ -278,15 +278,15 @@ class Problem : public std::enable_shared_from_this<Problem>
// Covariances -------------------------------------------
void clearCovariance();
- void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXs& _cov);
- void addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _cov);
- bool getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXs& _cov, const int _row = 0, const int _col=0) const;
- bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov) const;
- bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col = 0) const;
- bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance) const;
- bool getLastKeyFrameCovariance(Eigen::MatrixXs& _covariance) const;
- bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& _covariance) const;
- bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance) const;
+ void addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXd& _cov);
+ void addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _cov);
+ bool getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXd& _cov, const int _row = 0, const int _col=0) const;
+ bool getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const;
+ bool getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col = 0) const;
+ bool getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const;
+ bool getLastKeyFrameCovariance(Eigen::MatrixXd& _covariance) const;
+ bool getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& _covariance) const;
+ bool getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const;
// Solver management ----------------------------------------
diff --git a/include/core/processor/motion_buffer.h b/include/core/processor/motion_buffer.h
index 9d7ae613ffa3d5e065c0fe96b319a78d1441c2ae..a5b41ebfe3ebf77f269148d42781b7dd96cd36a2 100644
--- a/include/core/processor/motion_buffer.h
+++ b/include/core/processor/motion_buffer.h
@@ -23,28 +23,28 @@ struct Motion
public:
SizeEigen data_size_, delta_size_, cov_size_, calib_size_;
TimeStamp ts_; ///< Time stamp
- Eigen::VectorXs data_; ///< instantaneous motion data
- Eigen::MatrixXs data_cov_; ///< covariance of the instantaneous data
- Eigen::VectorXs delta_; ///< instantaneous motion delta
- Eigen::MatrixXs delta_cov_; ///< covariance of the instantaneous delta
- Eigen::VectorXs delta_integr_; ///< the integrated motion or delta-integral
- Eigen::MatrixXs delta_integr_cov_; ///< covariance of the integrated delta
- Eigen::MatrixXs jacobian_delta_; ///< Jacobian of the integration wrt delta_
- Eigen::MatrixXs jacobian_delta_integr_; ///< Jacobian of the integration wrt delta_integr_
- Eigen::MatrixXs jacobian_calib_; ///< Jacobian of delta_integr wrt extra states (TBD by the derived processors)
+ Eigen::VectorXd data_; ///< instantaneous motion data
+ Eigen::MatrixXd data_cov_; ///< covariance of the instantaneous data
+ Eigen::VectorXd delta_; ///< instantaneous motion delta
+ Eigen::MatrixXd delta_cov_; ///< covariance of the instantaneous delta
+ Eigen::VectorXd delta_integr_; ///< the integrated motion or delta-integral
+ Eigen::MatrixXd delta_integr_cov_; ///< covariance of the integrated delta
+ Eigen::MatrixXd jacobian_delta_; ///< Jacobian of the integration wrt delta_
+ Eigen::MatrixXd jacobian_delta_integr_; ///< Jacobian of the integration wrt delta_integr_
+ Eigen::MatrixXd jacobian_calib_; ///< Jacobian of delta_integr wrt extra states (TBD by the derived processors)
public:
Motion() = delete; // completely delete unpredictable stuff like this
Motion(const TimeStamp& _ts, SizeEigen _data_size, SizeEigen _delta_size, SizeEigen _cov_size, SizeEigen _calib_size);
Motion(const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _delta,
- const MatrixXs& _delta_cov,
- const VectorXs& _delta_int,
- const MatrixXs& _delta_integr_cov,
- const MatrixXs& _jac_delta,
- const MatrixXs& _jac_delta_int,
- const MatrixXs& _jacobian_calib);
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _delta,
+ const MatrixXd& _delta_cov,
+ const VectorXd& _delta_int,
+ const MatrixXd& _delta_integr_cov,
+ const MatrixXd& _jac_delta,
+ const MatrixXd& _jac_delta_int,
+ const MatrixXd& _jacobian_calib);
~Motion();
private:
void resize(SizeEigen _data_s, SizeEigen _delta_s, SizeEigen _delta_cov_s, SizeEigen _calib_s);
diff --git a/include/core/processor/processor_diff_drive.h b/include/core/processor/processor_diff_drive.h
index b6856afff3f5e5ab8a28c390e09fc6c3fda11805..05b98e4b517e25cd4aa476238bde867c2196de23 100644
--- a/include/core/processor/processor_diff_drive.h
+++ b/include/core/processor/processor_diff_drive.h
@@ -42,20 +42,20 @@ class ProcessorDiffDrive : public ProcessorOdom2D
protected:
// Motion integration
- virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
+ virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
const double _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) const override;
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _calib,
- const VectorXs& _calib_preint,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin) override;
virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_own) override;
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature_motion,
diff --git a/include/core/processor/processor_motion.h b/include/core/processor/processor_motion.h
index 620189d64d39af37549de4a8d246128ff9336653..88ee84a24470cc715adc8bf512db0d8004e827b0 100644
--- a/include/core/processor/processor_motion.h
+++ b/include/core/processor/processor_motion.h
@@ -99,7 +99,7 @@ struct ProcessorParamsMotion : public ProcessorParamsBase
* - The two operations are performed by the pure virtual method data2delta(). A possible implementation
* of data2delta() could be (we use the data member delta_ as the return value):
* \code
- * void data2delta(const VectorXs _data)
+ * void data2delta(const VectorXd _data)
* {
* delta_S = format(_data);
* delta_R = fromSensorFrame(delta_S);
@@ -164,13 +164,13 @@ class ProcessorMotion : public ProcessorBase
/** \brief Fill a reference to the state integrated so far
* \param _x the returned state vector
*/
- void getCurrentState(Eigen::VectorXs& _x) const;
+ void getCurrentState(Eigen::VectorXd& _x) const;
void getCurrentTimeStamp(TimeStamp& _ts) const { _ts = getBuffer().get().back().ts_; }
/** \brief Get the state integrated so far
* \return the state vector
*/
- Eigen::VectorXs getCurrentState() const;
+ Eigen::VectorXd getCurrentState() const;
TimeStamp getCurrentTimeStamp() const;
/** \brief Fill the state corresponding to the provided time-stamp
@@ -178,18 +178,18 @@ class ProcessorMotion : public ProcessorBase
* \param _x the returned state
* \return if state in the provided time-stamp could be resolved
*/
- bool getState(const TimeStamp& _ts, Eigen::VectorXs& _x) const;
+ bool getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const;
/** \brief Get the state corresponding to the provided time-stamp
* \param _ts the time stamp
* \return the state vector
*/
- Eigen::VectorXs getState(const TimeStamp& _ts) const;
+ Eigen::VectorXd getState(const TimeStamp& _ts) const;
/** \brief Gets the delta preintegrated covariance from all integrations so far
* \return the delta preintegrated covariance matrix
*/
- const Eigen::MatrixXs getCurrentDeltaPreintCov() const;
+ const Eigen::MatrixXd getCurrentDeltaPreintCov() const;
/** \brief Provide the motion integrated so far
* \return the integrated motion
@@ -215,7 +215,7 @@ class ProcessorMotion : public ProcessorBase
* \param _x_origin the state at the origin
* \param _ts_origin origin timestamp.
*/
- FrameBasePtr setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin);
+ FrameBasePtr setOrigin(const Eigen::VectorXd& _x_origin, const TimeStamp& _ts_origin);
MotionBuffer& getBuffer();
@@ -319,7 +319,7 @@ class ProcessorMotion : public ProcessorBase
*
* The delta-state format must be compatible for integration using
* the composition functions doing the math in this class: statePlusDelta() and deltaPlusDelta().
- * See the class documentation for some Eigen::VectorXs suggestions.
+ * See the class documentation for some Eigen::VectorXd suggestions.
*
* The data format is generally not the same as the delta format,
* because it is the format of the raw data provided by the Capture,
@@ -353,13 +353,13 @@ class ProcessorMotion : public ProcessorBase
*
* where `F_data = d_f/d_data` is the Jacobian of `f()`.
*/
- virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
+ virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
const double _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) const = 0;
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const = 0;
/** \brief composes a delta-state on top of another delta-state
* \param _delta1 the first delta-state
@@ -369,10 +369,10 @@ class ProcessorMotion : public ProcessorBase
*
* This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2.
*/
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
+ virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
const double _dt2,
- Eigen::VectorXs& _delta1_plus_delta2) const = 0;
+ Eigen::VectorXd& _delta1_plus_delta2) const = 0;
/** \brief composes a delta-state on top of another delta-state, and computes the Jacobians
* \param _delta1 the first delta-state
@@ -384,12 +384,12 @@ class ProcessorMotion : public ProcessorBase
*
* This function implements the composition (+) so that _delta1_plus_delta2 = _delta1 (+) _delta2 and its jacobians.
*/
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
+ virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
const double _dt2,
- Eigen::VectorXs& _delta1_plus_delta2,
- Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2) const = 0;
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const = 0;
/** \brief composes a delta-state on top of a state
* \param _x the initial state
@@ -399,20 +399,20 @@ class ProcessorMotion : public ProcessorBase
*
* This function implements the composition (+) so that _x2 = _x1 (+) _delta.
*/
- virtual void statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
+ virtual void statePlusDelta(const Eigen::VectorXd& _x,
+ const Eigen::VectorXd& _delta,
const double _dt,
- Eigen::VectorXs& _x_plus_delta) const = 0;
+ Eigen::VectorXd& _x_plus_delta) const = 0;
/** \brief Delta zero
* \return a delta state equivalent to the null motion.
*
- * Examples (see documentation of the the class for info on Eigen::VectorXs):
- * - 2D odometry: a 3-vector with all zeros, e.g. VectorXs::Zero(3)
+ * Examples (see documentation of the the class for info on Eigen::VectorXd):
+ * - 2D odometry: a 3-vector with all zeros, e.g. VectorXd::Zero(3)
* - 3D odometry: delta type is a PQ vector: 7-vector with [0,0,0, 0,0,0,1]
* - IMU: PQVBB 10-vector with [0,0,0, 0,0,0,1, 0,0,0] // No biases in the delta !!
*/
- virtual Eigen::VectorXs deltaZero() const = 0;
+ virtual Eigen::VectorXd deltaZero() const = 0;
/** \brief emplace a CaptureMotion
@@ -426,10 +426,10 @@ class ProcessorMotion : public ProcessorBase
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _calib,
- const VectorXs& _calib_preint,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin_ptr) = 0;
/** \brief emplace a feature corresponding to given capture and add the feature to this capture
@@ -477,17 +477,17 @@ class ProcessorMotion : public ProcessorBase
protected:
// helpers to avoid allocation
double dt_; ///< Time step
- Eigen::VectorXs x_; ///< current state
- Eigen::VectorXs delta_; ///< current delta
- Eigen::MatrixXs delta_cov_; ///< current delta covariance
- Eigen::VectorXs delta_integrated_; ///< integrated delta
- Eigen::MatrixXs delta_integrated_cov_; ///< integrated delta covariance
- Eigen::VectorXs calib_preint_; ///< calibration vector used during pre-integration
- Eigen::MatrixXs jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
- Eigen::MatrixXs jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
- Eigen::MatrixXs jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
- Eigen::MatrixXs jacobian_delta_calib_; ///< jacobian of delta wrt calib params
- Eigen::MatrixXs unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
+ Eigen::VectorXd x_; ///< current state
+ Eigen::VectorXd delta_; ///< current delta
+ Eigen::MatrixXd delta_cov_; ///< current delta covariance
+ Eigen::VectorXd delta_integrated_; ///< integrated delta
+ Eigen::MatrixXd delta_integrated_cov_; ///< integrated delta covariance
+ Eigen::VectorXd calib_preint_; ///< calibration vector used during pre-integration
+ Eigen::MatrixXd jacobian_delta_preint_; ///< jacobian of delta composition w.r.t previous delta integrated
+ Eigen::MatrixXd jacobian_delta_; ///< jacobian of delta composition w.r.t current delta
+ Eigen::MatrixXd jacobian_calib_; ///< jacobian of delta preintegration wrt calibration params
+ Eigen::MatrixXd jacobian_delta_calib_; ///< jacobian of delta wrt calib params
+ Eigen::MatrixXd unmeasured_perturbation_cov_; ///< Covariance of unmeasured DoF to avoid singularity
};
}
@@ -506,9 +506,9 @@ inline bool ProcessorMotion::voteForKeyFrame() const
return false;
}
-inline Eigen::VectorXs ProcessorMotion::getState(const TimeStamp& _ts) const
+inline Eigen::VectorXd ProcessorMotion::getState(const TimeStamp& _ts) const
{
- Eigen::VectorXs x(getProblem()->getFrameStructureSize());
+ Eigen::VectorXd x(getProblem()->getFrameStructureSize());
getState(_ts, x);
return x;
}
@@ -518,14 +518,14 @@ inline TimeStamp ProcessorMotion::getCurrentTimeStamp() const
return getBuffer().get().back().ts_;
}
-inline Eigen::VectorXs ProcessorMotion::getCurrentState() const
+inline Eigen::VectorXd ProcessorMotion::getCurrentState() const
{
- Eigen::VectorXs x(getProblem()->getFrameStructureSize());
+ Eigen::VectorXd x(getProblem()->getFrameStructureSize());
getCurrentState(x);
return x;
}
-inline void ProcessorMotion::getCurrentState(Eigen::VectorXs& _x) const
+inline void ProcessorMotion::getCurrentState(Eigen::VectorXd& _x) const
{
// ensure integrity
assert(origin_ptr_ && "Trying to access origin_ptr_ but it is nullptr!");
@@ -538,7 +538,7 @@ inline void ProcessorMotion::getCurrentState(Eigen::VectorXs& _x) const
statePlusDelta(origin_ptr_->getFrame()->getState(), last_ptr_->getDeltaCorrected(origin_ptr_->getCalibration()), Dt, _x);
}
-inline const Eigen::MatrixXs ProcessorMotion::getCurrentDeltaPreintCov() const
+inline const Eigen::MatrixXd ProcessorMotion::getCurrentDeltaPreintCov() const
{
return getBuffer().get().back().delta_integr_cov_;
}
@@ -579,15 +579,15 @@ inline MotionBuffer& ProcessorMotion::getBuffer()
inline Motion ProcessorMotion::motionZero(const TimeStamp& _ts) const
{
return Motion(_ts,
- VectorXs::Zero(data_size_), // data
- Eigen::MatrixXs::Zero(data_size_, data_size_), // Cov data
+ VectorXd::Zero(data_size_), // data
+ Eigen::MatrixXd::Zero(data_size_, data_size_), // Cov data
deltaZero(),
- Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_), // Cov delta
+ Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta
deltaZero(),
- Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_), // Cov delta_integr
- Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_), // Jac delta
- Eigen::MatrixXs::Zero(delta_cov_size_, delta_cov_size_), // Jac delta_integr
- Eigen::MatrixXs::Zero(delta_cov_size_, calib_size_) // Jac calib
+ Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Cov delta_integr
+ Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta
+ Eigen::MatrixXd::Zero(delta_cov_size_, delta_cov_size_), // Jac delta_integr
+ Eigen::MatrixXd::Zero(delta_cov_size_, calib_size_) // Jac calib
);
}
diff --git a/include/core/processor/processor_odom_2D.h b/include/core/processor/processor_odom_2D.h
index 6495930115e33f20091b4d03aaa7610061255478..6770f5e2464065169e86a9def4bf67b8a2af8636 100644
--- a/include/core/processor/processor_odom_2D.h
+++ b/include/core/processor/processor_odom_2D.h
@@ -50,36 +50,36 @@ class ProcessorOdom2D : public ProcessorMotion
virtual bool voteForKeyFrame() const override;
protected:
- virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
+ virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
const double _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) const override;
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
const double _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2) const override;
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override;
+ virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
const double _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2,
- Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2) const override;
- virtual void statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override;
+ virtual void statePlusDelta(const Eigen::VectorXd& _x,
+ const Eigen::VectorXd& _delta,
const double _Dt,
- Eigen::VectorXs& _x_plus_delta) const override;
- virtual Eigen::VectorXs deltaZero() const override;
+ Eigen::VectorXd& _x_plus_delta) const override;
+ virtual Eigen::VectorXd deltaZero() const override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _calib,
- const VectorXs& _calib_preint,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin_ptr) override;
virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
virtual FactorBasePtr emplaceFactor(FeatureBasePtr _feature,
@@ -90,9 +90,9 @@ class ProcessorOdom2D : public ProcessorMotion
};
-inline Eigen::VectorXs ProcessorOdom2D::deltaZero() const
+inline Eigen::VectorXd ProcessorOdom2D::deltaZero() const
{
- return Eigen::VectorXs::Zero(delta_size_);
+ return Eigen::VectorXd::Zero(delta_size_);
}
} // namespace wolf
diff --git a/include/core/processor/processor_odom_3D.h b/include/core/processor/processor_odom_3D.h
index ba6308be208bb93106a0fd0593cd73147bb2f789..9c487a8ea061fa42466fb6bf62fbc9868c3094de 100644
--- a/include/core/processor/processor_odom_3D.h
+++ b/include/core/processor/processor_odom_3D.h
@@ -69,36 +69,36 @@ class ProcessorOdom3D : public ProcessorMotion
public:
// Motion integration
- virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
+ virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
const double _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) const override;
- void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
const double _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2) const override;
- void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
+ Eigen::VectorXd& _delta1_plus_delta2) const override;
+ void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
const double _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2,
- Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2) const override;
- void statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const override;
+ void statePlusDelta(const Eigen::VectorXd& _x,
+ const Eigen::VectorXd& _delta,
const double _Dt,
- Eigen::VectorXs& _x_plus_delta) const override;
- Eigen::VectorXs deltaZero() const override;
+ Eigen::VectorXd& _x_plus_delta) const override;
+ Eigen::VectorXd deltaZero() const override;
bool voteForKeyFrame() const override;
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _calib,
- const VectorXs& _calib_preint,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin) override;
virtual FeatureBasePtr emplaceFeature(CaptureMotionPtr _capture_motion) override;
@@ -118,9 +118,9 @@ class ProcessorOdom3D : public ProcessorMotion
};
-inline Eigen::VectorXs ProcessorOdom3D::deltaZero() const
+inline Eigen::VectorXd ProcessorOdom3D::deltaZero() const
{
- return (Eigen::VectorXs(7) << 0,0,0, 0,0,0,1).finished(); // p, q
+ return (Eigen::VectorXd(7) << 0,0,0, 0,0,0,1).finished(); // p, q
}
} // namespace wolf
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index 6393a71aa3c9a83facec9d9166a48f04ebf01b0e..4b054ea46dc20fc220a81bae9dce7d639af6250e 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -28,7 +28,7 @@ namespace wolf {
* In order to use this macro, the derived sensor class, SensorClass,
* must have a constructor available with the API:
*
- * SensorClass(const VectorXs & _extrinsics, const IntrinsicsClassPtr _intrinsics);
+ * SensorClass(const VectorXd & _extrinsics, const IntrinsicsClassPtr _intrinsics);
*
* We recommend writing one of such constructors in your derived sensors.
*/
@@ -36,7 +36,7 @@ namespace wolf {
static \
SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _server) \
{ \
- Eigen::VectorXs extrinsics = _server.template getParam<Eigen::VectorXs>("sensor/" + _unique_name + "/extrinsic/pose"); \
+ Eigen::VectorXd extrinsics = _server.template getParam<Eigen::VectorXd>("sensor/" + _unique_name + "/extrinsic/pose"); \
\
assert(extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \
\
@@ -50,7 +50,7 @@ SensorBasePtr create(const std::string& _unique_name, const ParamsServer& _serve
} \
\
static \
-SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXs& _extrinsics, const IntrinsicsBasePtr _intrinsics) \
+SensorBasePtr create(const std::string& _unique_name, const Eigen::VectorXd& _extrinsics, const IntrinsicsBasePtr _intrinsics) \
{ \
assert(_extrinsics.size() == ExtrinsicsSize && "Bad extrinsics vector length."); \
\
@@ -101,8 +101,8 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
bool intrinsic_dynamic_;// intrinsic parameters vary with time? If so, they will be taken from the Capture nodes.
bool has_capture_; // indicates this sensor took at least one capture
- Eigen::VectorXs noise_std_; // std of sensor noise
- Eigen::MatrixXs noise_cov_; // cov matrix of noise
+ Eigen::VectorXd noise_std_; // std of sensor noise
+ Eigen::MatrixXd noise_cov_; // cov matrix of noise
std::map<unsigned int,FeatureBasePtr> params_prior_map_; // Priors (value and covariance) of extrinsic & intrinsic state blocks (by index in state_block_vec_)
@@ -143,7 +143,7 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr,
- const Eigen::VectorXs & _noise_std,
+ const Eigen::VectorXd & _noise_std,
const bool _extr_dyn = false,
const bool _intr_dyn = false);
@@ -214,23 +214,23 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
*
**/
void addPriorParameter(const unsigned int _i,
- const Eigen::VectorXs& _x,
- const Eigen::MatrixXs& _cov,
+ const Eigen::VectorXd& _x,
+ const Eigen::MatrixXd& _cov,
unsigned int _start_idx = 0,
int _size = -1);
- void addPriorP(const Eigen::VectorXs& _x,
- const Eigen::MatrixXs& _cov,
+ void addPriorP(const Eigen::VectorXd& _x,
+ const Eigen::MatrixXd& _cov,
unsigned int _start_idx = 0,
int _size = -1);
- void addPriorO(const Eigen::VectorXs& _x,
- const Eigen::MatrixXs& _cov);
- void addPriorIntrinsics(const Eigen::VectorXs& _x,
- const Eigen::MatrixXs& _cov,
+ void addPriorO(const Eigen::VectorXd& _x,
+ const Eigen::MatrixXd& _cov);
+ void addPriorIntrinsics(const Eigen::VectorXd& _x,
+ const Eigen::MatrixXd& _cov,
unsigned int _start_idx = 0,
int _size = -1);
SizeEigen getCalibSize() const;
- Eigen::VectorXs getCalibration() const;
+ Eigen::VectorXd getCalibration() const;
bool isExtrinsicDynamic() const;
bool isIntrinsicDynamic() const;
@@ -239,10 +239,10 @@ class SensorBase : public NodeBase, public std::enable_shared_from_this<SensorBa
bool extrinsicsInCaptures() const { return extrinsic_dynamic_ && has_capture_; }
bool intrinsicsInCaptures() const { return intrinsic_dynamic_ && has_capture_; }
- void setNoiseStd(const Eigen::VectorXs & _noise_std);
- void setNoiseCov(const Eigen::MatrixXs & _noise_std);
- Eigen::VectorXs getNoiseStd() const;
- Eigen::MatrixXs getNoiseCov() const;
+ void setNoiseStd(const Eigen::VectorXd & _noise_std);
+ void setNoiseCov(const Eigen::MatrixXd & _noise_std);
+ Eigen::VectorXd getNoiseStd() const;
+ Eigen::MatrixXd getNoiseCov() const;
void setExtrinsicDynamic(bool _extrinsic_dynamic);
void setIntrinsicDynamic(bool _intrinsic_dynamic);
@@ -325,12 +325,12 @@ inline bool SensorBase::isIntrinsicDynamic() const
return intrinsic_dynamic_;
}
-inline Eigen::VectorXs SensorBase::getNoiseStd() const
+inline Eigen::VectorXd SensorBase::getNoiseStd() const
{
return noise_std_;
}
-inline Eigen::MatrixXs SensorBase::getNoiseCov() const
+inline Eigen::MatrixXd SensorBase::getNoiseCov() const
{
return noise_cov_;
}
@@ -360,17 +360,17 @@ inline void SensorBase::setHardware(const HardwareBasePtr _hw_ptr)
hardware_ptr_ = _hw_ptr;
}
-inline void SensorBase::addPriorP(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+inline void SensorBase::addPriorP(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
{
addPriorParameter(0, _x, _cov, _start_idx, _size);
}
-inline void SensorBase::addPriorO(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov)
+inline void SensorBase::addPriorO(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov)
{
addPriorParameter(1, _x, _cov);
}
-inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+inline void SensorBase::addPriorIntrinsics(const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
{
addPriorParameter(2, _x, _cov);
}
diff --git a/include/core/sensor/sensor_diff_drive.h b/include/core/sensor/sensor_diff_drive.h
index ae1bf5846263099e71f09c483adc2f8bdd2400bc..96af457291dbbbdf6737a5f13d7544e5091611bf 100644
--- a/include/core/sensor/sensor_diff_drive.h
+++ b/include/core/sensor/sensor_diff_drive.h
@@ -48,7 +48,7 @@ WOLF_PTR_TYPEDEFS(SensorDiffDrive);
class SensorDiffDrive : public SensorBase
{
public:
- SensorDiffDrive(const Eigen::VectorXs& _extrinsics,
+ SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
IntrinsicsDiffDrivePtr _intrinsics);
WOLF_SENSOR_CREATE(SensorDiffDrive, IntrinsicsDiffDrive, 3);
virtual ~SensorDiffDrive();
diff --git a/include/core/sensor/sensor_factory.h b/include/core/sensor/sensor_factory.h
index 3c74b0fd7ad1d32d5b3d8dc8cf867699ea832f80..3521a63c91c2e69086876401a57745371d480819 100644
--- a/include/core/sensor/sensor_factory.h
+++ b/include/core/sensor/sensor_factory.h
@@ -79,7 +79,7 @@ namespace wolf
* that can be derived for each derived sensor:
*
* \code
- * static SensorBasePtr create(const std::string& _name, Eigen::VectorXs& _extrinsics_pq, IntrinsicsBasePtr _intrinsics)
+ * static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _extrinsics_pq, IntrinsicsBasePtr _intrinsics)
* \endcode
*
* See further down for an implementation example.
@@ -124,7 +124,7 @@ namespace wolf
* Here is an example of SensorCamera::create() extracted from sensor_camera.cpp:
*
* \code
- * static SensorBasePtr create(const std::string& _name, Eigen::VectorXs& _extrinsics_pq, IntrinsicsBasePtr _intrinsics)
+ * static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _extrinsics_pq, IntrinsicsBasePtr _intrinsics)
* {
* // check extrinsics vector
* assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
@@ -188,7 +188,7 @@ namespace wolf
* // an extrinsics vector, and
* // a pointer to the intrinsics struct:
*
- * Eigen::VectorXs extrinsics_1(7); // give it some values...
+ * Eigen::VectorXd extrinsics_1(7); // give it some values...
* IntrinsicsCamera intrinsics_1({...}); // see IntrinsicsFactory to fill in the derived struct
*
* SensorBasePtr camera_1_ptr =
@@ -196,7 +196,7 @@ namespace wolf
*
* // A second camera... with a different name!
*
- * Eigen::VectorXs extrinsics_2(7);
+ * Eigen::VectorXd extrinsics_2(7);
* IntrinsicsCamera intrinsics_2({...});
*
* SensorBasePtr camera_2_ptr =
@@ -222,7 +222,7 @@ inline std::string IntrinsicsFactory::getClass()
// Sensor factory
typedef Factory<SensorBase,
const std::string&,
- const Eigen::VectorXs&, const IntrinsicsBasePtr> SensorFactory;
+ const Eigen::VectorXd&, const IntrinsicsBasePtr> SensorFactory;
template<>
inline std::string SensorFactory::getClass()
diff --git a/include/core/sensor/sensor_odom_2D.h b/include/core/sensor/sensor_odom_2D.h
index 2b742c55d089d5b4fb19f8ce7728b24abfbc4e21..b27d1d768310ef1d14baabcfbe2a9cb13960a080 100644
--- a/include/core/sensor/sensor_odom_2D.h
+++ b/include/core/sensor/sensor_odom_2D.h
@@ -42,8 +42,8 @@ class SensorOdom2D : public SensorBase
double k_rot_to_rot_; ///< ratio of rotation variance to rotation, for odometry noise calculation
public:
- SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics);
- SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics);
+ SensorOdom2D(Eigen::VectorXd _extrinsics, const IntrinsicsOdom2D& _intrinsics);
+ SensorOdom2D(Eigen::VectorXd _extrinsics, IntrinsicsOdom2DPtr _intrinsics);
WOLF_SENSOR_CREATE(SensorOdom2D, IntrinsicsOdom2D, 3);
virtual ~SensorOdom2D();
diff --git a/include/core/sensor/sensor_odom_3D.h b/include/core/sensor/sensor_odom_3D.h
index 50dfc31de3ef127c5a82613aa1fe2007025bd170..cd7493b33e0f29fd55cfa409b019a2c705d0b5f6 100644
--- a/include/core/sensor/sensor_odom_3D.h
+++ b/include/core/sensor/sensor_odom_3D.h
@@ -60,8 +60,8 @@ class SensorOdom3D : public SensorBase
double min_rot_var_;
public:
- SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsOdom3D& params);
- SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom3DPtr params);
+ SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, const IntrinsicsOdom3D& params);
+ SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, IntrinsicsOdom3DPtr params);
WOLF_SENSOR_CREATE(SensorOdom3D, IntrinsicsOdom3D, 7);
virtual ~SensorOdom3D();
diff --git a/include/core/solver/solver_factory.h b/include/core/solver/solver_factory.h
index 95bab65a735c36946f610ad535fbab43c0516f19..64d980c46a1a4a27b9af99feba7cd6952aeb0a55 100644
--- a/include/core/solver/solver_factory.h
+++ b/include/core/solver/solver_factory.h
@@ -79,7 +79,7 @@ namespace wolf
* that can be derived for each derived sensor:
*
* \code
- * static SensorBasePtr create(const std::string& _name, Eigen::VectorXs& _extrinsics_pq, IntrinsicsBasePtr _intrinsics)
+ * static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _extrinsics_pq, IntrinsicsBasePtr _intrinsics)
* \endcode
*
* See further down for an implementation example.
@@ -124,7 +124,7 @@ namespace wolf
* Here is an example of SensorCamera::create() extracted from sensor_camera.cpp:
*
* \code
- * static SensorBasePtr create(const std::string& _name, Eigen::VectorXs& _extrinsics_pq, IntrinsicsBasePtr _intrinsics)
+ * static SensorBasePtr create(const std::string& _name, Eigen::VectorXd& _extrinsics_pq, IntrinsicsBasePtr _intrinsics)
* {
* // check extrinsics vector
* assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector length. Should be 7 for 3D.");
@@ -188,7 +188,7 @@ namespace wolf
* // an extrinsics vector, and
* // a pointer to the intrinsics struct:
*
- * Eigen::VectorXs extrinsics_1(7); // give it some values...
+ * Eigen::VectorXd extrinsics_1(7); // give it some values...
* IntrinsicsCamera intrinsics_1({...}); // see IntrinsicsFactory to fill in the derived struct
*
* SensorBasePtr camera_1_ptr =
@@ -196,7 +196,7 @@ namespace wolf
*
* // A second camera... with a different name!
*
- * Eigen::VectorXs extrinsics_2(7);
+ * Eigen::VectorXd extrinsics_2(7);
* IntrinsicsCamera intrinsics_2({...});
*
* SensorBasePtr camera_2_ptr =
diff --git a/include/core/solver/solver_manager.h b/include/core/solver/solver_manager.h
index 297b06a37cdc8ec5834341051ee162a85188c1b9..3d25b6fd79163f0cc13ade098122cdc41a5b4cd6 100644
--- a/include/core/solver/solver_manager.h
+++ b/include/core/solver/solver_manager.h
@@ -73,9 +73,9 @@ public:
protected:
- std::map<StateBlockPtr, Eigen::VectorXs> state_blocks_;
+ std::map<StateBlockPtr, Eigen::VectorXd> state_blocks_;
- virtual Eigen::VectorXs& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
+ virtual Eigen::VectorXd& getAssociatedMemBlock(const StateBlockPtr& state_ptr);
virtual double* getAssociatedMemBlockPtr(const StateBlockPtr& state_ptr);
virtual std::string solveImpl(const ReportVerbosity report_level) = 0;
diff --git a/include/core/solver_suitesparse/cost_function_base.h b/include/core/solver_suitesparse/cost_function_base.h
index bf8f4ebcbee58431e0bb741eded9d3c3c799fdbb..523e2147ace0242d5b4739480a59912686fd07cd 100644
--- a/include/core/solver_suitesparse/cost_function_base.h
+++ b/include/core/solver_suitesparse/cost_function_base.h
@@ -15,18 +15,18 @@ class CostFunctionBase
{
protected:
unsigned int n_blocks_;
- Eigen::MatrixXs J_0_;
- Eigen::MatrixXs J_1_;
- Eigen::MatrixXs J_2_;
- Eigen::MatrixXs J_3_;
- Eigen::MatrixXs J_4_;
- Eigen::MatrixXs J_5_;
- Eigen::MatrixXs J_6_;
- Eigen::MatrixXs J_7_;
- Eigen::MatrixXs J_8_;
- Eigen::MatrixXs J_9_;
- Eigen::VectorXs residual_;
- std::vector<Eigen::MatrixXs*> jacobians_;
+ Eigen::MatrixXd J_0_;
+ Eigen::MatrixXd J_1_;
+ Eigen::MatrixXd J_2_;
+ Eigen::MatrixXd J_3_;
+ Eigen::MatrixXd J_4_;
+ Eigen::MatrixXd J_5_;
+ Eigen::MatrixXd J_6_;
+ Eigen::MatrixXd J_7_;
+ Eigen::MatrixXd J_8_;
+ Eigen::MatrixXd J_9_;
+ Eigen::VectorXd residual_;
+ std::vector<Eigen::MatrixXd*> jacobians_;
std::vector<unsigned int> block_sizes_;
public:
@@ -75,18 +75,18 @@ class CostFunctionBase
virtual void evaluateResidualJacobians() = 0;
- void getResidual(Eigen::VectorXs &residual)
+ void getResidual(Eigen::VectorXd &residual)
{
residual.resize(residual_.size());
residual = residual_;
}
- std::vector<Eigen::MatrixXs*> getJacobians()
+ std::vector<Eigen::MatrixXd*> getJacobians()
{
return jacobians_;
}
- void getJacobians(std::vector<Eigen::MatrixXs>& jacobians)
+ void getJacobians(std::vector<Eigen::MatrixXd>& jacobians)
{
jacobians.resize(n_blocks_);
for (unsigned int i = 0; i<n_blocks_; i++)
diff --git a/include/core/solver_suitesparse/qr_solver.h b/include/core/solver_suitesparse/qr_solver.h
index 452971da53b36126d1b9c5b9492d99357dcd6ff0..53ea301cff2f9506c6978dc401bc2b65c14df961 100644
--- a/include/core/solver_suitesparse/qr_solver.h
+++ b/include/core/solver_suitesparse/qr_solver.h
@@ -162,8 +162,8 @@ class SolverQR
unsigned int meas_dim = _factor_ptr->getSize();
- std::vector<Eigen::MatrixXs> jacobians(_factor_ptr->getStateBlockPtrVector().size());
- Eigen::VectorXs error(meas_dim);
+ std::vector<Eigen::MatrixXd> jacobians(_factor_ptr->getStateBlockPtrVector().size());
+ Eigen::VectorXd error(meas_dim);
cost_functions_.back()->evaluateResidualJacobians();
cost_functions_.back()->getResidual(error);
@@ -423,7 +423,7 @@ class SolverQR
// UPDATE X VALUE
for (unsigned int i = 0; i < nodes_.size(); i++)
{
- Eigen::Map < Eigen::VectorXs > x_i(nodes_.at(i)->get(), nodes_.at(i)->getSize());
+ Eigen::Map < Eigen::VectorXd > x_i(nodes_.at(i)->get(), nodes_.at(i)->getSize());
x_i += x_incr_.segment(nodeLocation(i), nodes_.at(i)->getSize());
}
// Zero the error
diff --git a/include/core/solver_suitesparse/sparse_utils.h b/include/core/solver_suitesparse/sparse_utils.h
index 6bc15f95a5300ec8c78bc8c7f941df57bab9d0d5..7f5e0a0d0d2a56c9c798509c8b092eba0a35d855 100644
--- a/include/core/solver_suitesparse/sparse_utils.h
+++ b/include/core/solver_suitesparse/sparse_utils.h
@@ -14,17 +14,17 @@
namespace wolf
{
-void eraseBlockRow(Eigen::SparseMatrixs& A, const unsigned int& _row, const unsigned int& _n_rows)
+void eraseBlockRow(Eigen::SparseMatrixd& A, const unsigned int& _row, const unsigned int& _n_rows)
{
A.prune([](int i, int, double) { return i >= _row && i < _row + _n_rows; });
}
-void eraseBlockCol(Eigen::SparseMatrixs& A, const unsigned int& _col, const unsigned int& _n_cols)
+void eraseBlockCol(Eigen::SparseMatrixd& A, const unsigned int& _col, const unsigned int& _n_cols)
{
A.prune([](int, int j, double) { return j >= _col && j < _col + _n_cols; });
}
-void addSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrixs& original, const unsigned int& row, const unsigned int& col)
+void addSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
{
for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
@@ -33,7 +33,7 @@ void addSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrixs& original,
original.makeCompressed();
}
-void insertSparseBlock(const Eigen::MatrixXs& ins, Eigen::SparseMatrixs& original, const unsigned int& row, const unsigned int& col)
+void insertSparseBlock(const Eigen::MatrixXd& ins, Eigen::SparseMatrixd& original, const unsigned int& row, const unsigned int& col)
{
for (auto ins_row = 0; ins_row < ins.rows(); ins_row++)
for (auto ins_col = 0; ins_col < ins.cols(); ins_col++)
diff --git a/include/core/state_block/local_parametrization_angle.h b/include/core/state_block/local_parametrization_angle.h
index 5b146d84d3c46de5f4b6699fdb0efb83edb64c9d..6c063da65a74ae9234a61a1583520f4fffbc8e17 100644
--- a/include/core/state_block/local_parametrization_angle.h
+++ b/include/core/state_block/local_parametrization_angle.h
@@ -20,13 +20,13 @@ class LocalParametrizationAngle : public LocalParametrizationBase
LocalParametrizationAngle();
virtual ~LocalParametrizationAngle();
- virtual bool plus(Eigen::Map<const Eigen::VectorXs>& _h, Eigen::Map<const Eigen::VectorXs>& _delta,
- Eigen::Map<Eigen::VectorXs>& _h_plus_delta) const;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXs>& _h,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const;
- virtual bool minus(Eigen::Map<const Eigen::VectorXs>& _x1,
- Eigen::Map<const Eigen::VectorXs>& _x2,
- Eigen::Map<Eigen::VectorXs>& _x2_minus_x1);
+ virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<const Eigen::VectorXd>& _delta,
+ Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const;
+ virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<Eigen::MatrixXd>& _jacobian) const;
+ virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
+ Eigen::Map<const Eigen::VectorXd>& _x2,
+ Eigen::Map<Eigen::VectorXd>& _x2_minus_x1);
};
@@ -41,24 +41,24 @@ inline LocalParametrizationAngle::~LocalParametrizationAngle()
//
}
-inline bool LocalParametrizationAngle::plus(Eigen::Map<const Eigen::VectorXs>& _h,
- Eigen::Map<const Eigen::VectorXs>& _delta,
- Eigen::Map<Eigen::VectorXs>& _h_plus_delta) const
+inline bool LocalParametrizationAngle::plus(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<const Eigen::VectorXd>& _delta,
+ Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const
{
_h_plus_delta(0) = pi2pi(_h(0) + _delta(0));
return true;
}
-inline bool LocalParametrizationAngle::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _h,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
+inline bool LocalParametrizationAngle::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<Eigen::MatrixXd>& _jacobian) const
{
_jacobian(0) = 1.0;
return true;
}
-inline bool LocalParametrizationAngle::minus(Eigen::Map<const Eigen::VectorXs>& _x1,
- Eigen::Map<const Eigen::VectorXs>& _x2,
- Eigen::Map<Eigen::VectorXs>& _x2_minus_x1)
+inline bool LocalParametrizationAngle::minus(Eigen::Map<const Eigen::VectorXd>& _x1,
+ Eigen::Map<const Eigen::VectorXd>& _x2,
+ Eigen::Map<Eigen::VectorXd>& _x2_minus_x1)
{
_x2_minus_x1(0) = pi2pi(_x2(0)-_x1(0));
return true;
diff --git a/include/core/state_block/local_parametrization_base.h b/include/core/state_block/local_parametrization_base.h
index 94ed61f62d83736c27593afe151ab701e51e09e5..e7a8d2200376445124ebbb89ec0eafda17b7638d 100644
--- a/include/core/state_block/local_parametrization_base.h
+++ b/include/core/state_block/local_parametrization_base.h
@@ -22,13 +22,13 @@ class LocalParametrizationBase{
LocalParametrizationBase(unsigned int _global_size, unsigned int _local_size);
virtual ~LocalParametrizationBase();
- virtual bool plus(Eigen::Map<const Eigen::VectorXs>& _x,
- Eigen::Map<const Eigen::VectorXs>& _delta,
- Eigen::Map<Eigen::VectorXs>& _x_plus_delta) const = 0;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXs>& _x, Eigen::Map<Eigen::MatrixXs>& _jacobian) const = 0;
- virtual bool minus(Eigen::Map<const Eigen::VectorXs>& _x1,
- Eigen::Map<const Eigen::VectorXs>& _x2,
- Eigen::Map<Eigen::VectorXs>& _x2_minus_x1) = 0;
+ virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _x,
+ Eigen::Map<const Eigen::VectorXd>& _delta,
+ Eigen::Map<Eigen::VectorXd>& _x_plus_delta) const = 0;
+ virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _x, Eigen::Map<Eigen::MatrixXd>& _jacobian) const = 0;
+ virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _x1,
+ Eigen::Map<const Eigen::VectorXd>& _x2,
+ Eigen::Map<Eigen::VectorXd>& _x2_minus_x1) = 0;
unsigned int getLocalSize() const;
unsigned int getGlobalSize() const;
diff --git a/include/core/state_block/local_parametrization_homogeneous.h b/include/core/state_block/local_parametrization_homogeneous.h
index 4729bb3ff1f8e4ff821b4cc87f5c7c192b2ccaf0..31b3a82b9a13f789be6b255be8f4caca89a1ef4d 100644
--- a/include/core/state_block/local_parametrization_homogeneous.h
+++ b/include/core/state_block/local_parametrization_homogeneous.h
@@ -40,13 +40,13 @@ class LocalParametrizationHomogeneous : public LocalParametrizationBase
LocalParametrizationHomogeneous();
virtual ~LocalParametrizationHomogeneous();
- virtual bool plus(Eigen::Map<const Eigen::VectorXs>& _h,
- Eigen::Map<const Eigen::VectorXs>& _delta,
- Eigen::Map<Eigen::VectorXs>& _h_plus_delta) const;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXs>& _h, Eigen::Map<Eigen::MatrixXs>& _jacobian) const;
- virtual bool minus(Eigen::Map<const Eigen::VectorXs>& _h1,
- Eigen::Map<const Eigen::VectorXs>& _h2,
- Eigen::Map<Eigen::VectorXs>& _h2_minus_h1);
+ virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<const Eigen::VectorXd>& _delta,
+ Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const;
+ virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h, Eigen::Map<Eigen::MatrixXd>& _jacobian) const;
+ virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _h1,
+ Eigen::Map<const Eigen::VectorXd>& _h2,
+ Eigen::Map<Eigen::VectorXd>& _h2_minus_h1);
};
} // namespace wolf
diff --git a/include/core/state_block/local_parametrization_quaternion.h b/include/core/state_block/local_parametrization_quaternion.h
index 3076568d529835f12f414e0194fe2a0396e65305..32799f565b678c760d5540d5019a47e6e62bfd5b 100644
--- a/include/core/state_block/local_parametrization_quaternion.h
+++ b/include/core/state_block/local_parametrization_quaternion.h
@@ -64,15 +64,15 @@ public:
//
}
- virtual bool plus(Eigen::Map<const Eigen::VectorXs>& _q,
- Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const;
+ virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _q,
+ Eigen::Map<const Eigen::VectorXd>& _delta_theta,
+ Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXs>& _q,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const;
- virtual bool minus(Eigen::Map<const Eigen::VectorXs>& _q1,
- Eigen::Map<const Eigen::VectorXs>& _q2,
- Eigen::Map<Eigen::VectorXs>& _q2_minus_q1);
+ virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
+ Eigen::Map<Eigen::MatrixXd>& _jacobian) const;
+ virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _q1,
+ Eigen::Map<const Eigen::VectorXd>& _q2,
+ Eigen::Map<Eigen::VectorXd>& _q2_minus_q1);
};
typedef LocalParametrizationQuaternion<DQ_GLOBAL> LocalParametrizationQuaternionGlobal;
diff --git a/include/core/state_block/state_block.h b/include/core/state_block/state_block.h
index 01fa0bc31927ccabdc2e15788e2b8966bb2740e5..b68b4ad7dd482f071faf04339656879635232dd2 100644
--- a/include/core/state_block/state_block.h
+++ b/include/core/state_block/state_block.h
@@ -35,7 +35,7 @@ public:
std::atomic_bool fixed_; ///< Key to indicate whether the state is fixed or not
std::atomic<SizeEigen> state_size_; ///< State vector size
- Eigen::VectorXs state_; ///< State vector storing the state values
+ Eigen::VectorXd state_; ///< State vector storing the state values
mutable std::mutex mut_state_; ///< State vector mutex
LocalParametrizationBasePtr local_param_ptr_; ///< Local parametrization useful for optimizing in the tangent space to the manifold
@@ -59,7 +59,7 @@ public:
* \param _fixed Indicates this state is not estimated and thus acts as a fixed parameter
* \param _local_param_ptr pointer to the local parametrization for the block
**/
- StateBlock(const Eigen::VectorXs& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr);
+ StateBlock(const Eigen::VectorXd& _state, bool _fixed = false, LocalParametrizationBasePtr _local_param_ptr = nullptr);
///< Explicitly not copyable/movable
StateBlock(const StateBlock& o) = delete;
@@ -72,11 +72,11 @@ public:
/** \brief Returns the state vector
**/
- Eigen::VectorXs getState() const;
+ Eigen::VectorXd getState() const;
/** \brief Sets the state vector
**/
- void setState(const Eigen::VectorXs& _state, const bool _notify = true);
+ void setState(const Eigen::VectorXd& _state, const bool _notify = true);
/** \brief Returns the state size
**/
@@ -160,7 +160,7 @@ public:
namespace wolf {
-inline StateBlock::StateBlock(const Eigen::VectorXs& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
+inline StateBlock::StateBlock(const Eigen::VectorXd& _state, bool _fixed, LocalParametrizationBasePtr _local_param_ptr) :
// notifications_{Notification::ADD},
fixed_(_fixed),
state_size_(_state.size()),
@@ -177,7 +177,7 @@ inline StateBlock::StateBlock(const SizeEigen _size, bool _fixed, LocalParametri
// notifications_{Notification::ADD},
fixed_(_fixed),
state_size_(_size),
- state_(Eigen::VectorXs::Zero(_size)),
+ state_(Eigen::VectorXd::Zero(_size)),
local_param_ptr_(_local_param_ptr),
state_updated_(false),
fix_updated_(false),
@@ -192,7 +192,7 @@ inline StateBlock::~StateBlock()
// std::cout << "destructed -sb" << std::endl;
}
-inline Eigen::VectorXs StateBlock::getState() const
+inline Eigen::VectorXd StateBlock::getState() const
{
std::lock_guard<std::mutex> lock(mut_state_);
return state_;
diff --git a/include/core/state_block/state_homogeneous_3D.h b/include/core/state_block/state_homogeneous_3D.h
index c71e3c7f1be920e6e144af255893d9a3efc2aafe..9ae7e8da01e5d1544dd887682b7f164e1ddc1e99 100644
--- a/include/core/state_block/state_homogeneous_3D.h
+++ b/include/core/state_block/state_homogeneous_3D.h
@@ -17,11 +17,11 @@ class StateHomogeneous3D : public StateBlock
{
public:
StateHomogeneous3D(bool _fixed = false);
- StateHomogeneous3D(const Eigen::VectorXs _state, bool _fixed = false);
+ StateHomogeneous3D(const Eigen::VectorXd _state, bool _fixed = false);
virtual ~StateHomogeneous3D();
};
-inline StateHomogeneous3D::StateHomogeneous3D(const Eigen::VectorXs _state, bool _fixed) :
+inline StateHomogeneous3D::StateHomogeneous3D(const Eigen::VectorXd _state, bool _fixed) :
StateBlock(_state, _fixed)
{
assert(_state.size() == 4 && "Homogeneous 3D must be size 4.");
diff --git a/include/core/state_block/state_quaternion.h b/include/core/state_block/state_quaternion.h
index 66b23c0504339b98b1c9be1eeaafd9d552ecf472..3732201633ca04eeac5815b9948c8e9d7261f852 100644
--- a/include/core/state_block/state_quaternion.h
+++ b/include/core/state_block/state_quaternion.h
@@ -17,17 +17,17 @@ class StateQuaternion : public StateBlock
{
public:
StateQuaternion(bool _fixed = false);
- StateQuaternion(const Eigen::VectorXs& _state, bool _fixed = false);
- StateQuaternion(const Eigen::Quaternions& _quaternion, bool _fixed = false);
+ StateQuaternion(const Eigen::VectorXd& _state, bool _fixed = false);
+ StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed = false);
virtual ~StateQuaternion();
};
-inline StateQuaternion::StateQuaternion(const Eigen::Quaternions& _quaternion, bool _fixed) :
+inline StateQuaternion::StateQuaternion(const Eigen::Quaterniond& _quaternion, bool _fixed) :
StateBlock(_quaternion.coeffs(), _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
{
}
-inline StateQuaternion::StateQuaternion(const Eigen::VectorXs& _state, bool _fixed) :
+inline StateQuaternion::StateQuaternion(const Eigen::VectorXd& _state, bool _fixed) :
StateBlock(_state, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
{
assert(_state.size() == 4 && "The quaternion state vector must be of size 4");
@@ -36,7 +36,7 @@ inline StateQuaternion::StateQuaternion(const Eigen::VectorXs& _state, bool _fix
inline StateQuaternion::StateQuaternion(bool _fixed) :
StateBlock(4, _fixed, std::make_shared<LocalParametrizationQuaternion<DQ_LOCAL>>())
{
- state_ = Eigen::Quaternions::Identity().coeffs();
+ state_ = Eigen::Quaterniond::Identity().coeffs();
//
}
diff --git a/include/core/utils/eigen_assert.h b/include/core/utils/eigen_assert.h
index 5c079da7aab383eed93578869b219c65854419d4..7873b792cf89c4af8e015776798e381f0315776f 100644
--- a/include/core/utils/eigen_assert.h
+++ b/include/core/utils/eigen_assert.h
@@ -10,7 +10,7 @@ namespace Eigen {
*
* Help:
*
- * The WOLF project implements many template functions using Eigen Matrix and Quaternions, in different versions
+ * The WOLF project implements many template functions using Eigen Matrix and Quaterniond, in different versions
* (Static size, Dynamic size, Map, Matrix expression).
*
* Eigen provides some macros for STATIC assert of matrix sizes, the most common of them are (see Eigen's StaticAssert.h):
diff --git a/include/core/utils/eigen_predicates.h b/include/core/utils/eigen_predicates.h
index dabad84a65b2a5dab343d6809727f4b5ffe2ad4b..1f319f3f81df939415a688fd6e7206918f5d1825 100644
--- a/include/core/utils/eigen_predicates.h
+++ b/include/core/utils/eigen_predicates.h
@@ -18,10 +18,10 @@ namespace wolf {
/// defined as :
/// abs(x - y) <= (min)(abs(x), abs(y)) * prec
/// which does not play nice with y = 0
-auto is_constant = [](const Eigen::MatrixXs& lhs, const double val, const double precision)
+auto is_constant = [](const Eigen::MatrixXd& lhs, const double val, const double precision)
{
- for (Eigen::MatrixXs::Index j = 0; j < lhs.cols(); ++j)
- for (Eigen::MatrixXs::Index i = 0; i < lhs.rows(); ++i)
+ for (Eigen::MatrixXd::Index j = 0; j < lhs.cols(); ++j)
+ for (Eigen::MatrixXd::Index i = 0; i < lhs.rows(); ++i)
if (std::abs(lhs.coeff(i, j) - val) > precision)
return false;
return true;
@@ -29,31 +29,31 @@ auto is_constant = [](const Eigen::MatrixXs& lhs, const double val, const double
/// @brief check that each element of the Matrix/Vector difference
/// is approx 0 +- precision
-auto pred_zero = [](const Eigen::MatrixXs& lhs, const double precision)
+auto pred_zero = [](const Eigen::MatrixXd& lhs, const double precision)
{ return is_constant(lhs, 0, precision); };
/// @brief check that each element of the Matrix/Vector difference
/// is approx 0 +- precision
-auto pred_diff_zero = [](const Eigen::MatrixXs& lhs, const Eigen::MatrixXs& rhs, const double precision)
+auto pred_diff_zero = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
{ return pred_zero(lhs - rhs, precision); };
/// @brief check that the Matrix/Vector difference norm is approx 0 +- precision
-auto pred_diff_norm_zero = [](const Eigen::MatrixXs& lhs, const Eigen::MatrixXs& rhs, const double precision)
+auto pred_diff_norm_zero = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
{ return std::abs((lhs - rhs).norm()) <= std::abs(precision); };
/// @brief check that the lhs Matrix/Vector is elem-wise approx the rhs +-precision
-auto pred_is_approx = [](const Eigen::MatrixXs& lhs, const Eigen::MatrixXs& rhs, const double precision)
+auto pred_is_approx = [](const Eigen::MatrixXd& lhs, const Eigen::MatrixXd& rhs, const double precision)
{ return lhs.isApprox(rhs, precision); };
/// @brief check that angle θ of rotation required to get from lhs quaternion to rhs
/// is <= precision
-auto pred_quat_is_approx = [](const Eigen::Quaternions& lhs, const Eigen::Quaternions& rhs, const double precision)
+auto pred_quat_is_approx = [](const Eigen::Quaterniond& lhs, const Eigen::Quaterniond& rhs, const double precision)
{ return pred_zero( log_q(rhs * lhs.inverse()), precision ); };
/// @brief check that angle θ of rotation required to get from lhs quaternion to identity
/// is <= precision
-auto pred_quat_identity = [](const Eigen::Quaternions& lhs, const double precision)
- { return pred_quat_is_approx(lhs, Eigen::Quaternions::Identity(), precision); };
+auto pred_quat_identity = [](const Eigen::Quaterniond& lhs, const double precision)
+ { return pred_quat_is_approx(lhs, Eigen::Quaterniond::Identity(), precision); };
/// @brief check that rotation angle to get from lhs angle to rhs is <= precision
auto pred_angle_is_approx = [](const double lhs, const double rhs, const double precision)
@@ -81,13 +81,13 @@ auto pred_angle_zero = [](const double lhs, const double precision)
/// @see pred_zero for translation comparison
/// @see pred_quat_identity for 3d rotation comparison
/// @see pred_angle_zero for 2d rotation comparison
-//auto pred_pose_is_approx = [](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs, const double precision)
+//auto pred_pose_is_approx = [](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs, const double precision)
// {
-// const Eigen::MatrixXs d = lhs.inverse() * rhs;
+// const Eigen::MatrixXd d = lhs.inverse() * rhs;
// const bool tok = pred_zero(d.rightCols(1), precision);
// const bool qok = (lhs.rows() == 3)?
-// pred_quat_identity(Eigen::Quaternions(d.block(0,0,3,1)),
+// pred_quat_identity(Eigen::Quaterniond(d.block(0,0,3,1)),
// precision)
// : (lhs.rows() == 2)? pred_angle_zero(getYaw(d), precision)
// : throw std::runtime_error("Canno't compare pose in Dim > 3 !");
diff --git a/include/core/utils/utils_gtest.h b/include/core/utils/utils_gtest.h
index c296740b40c28805ba87cd5fd78e46f98ab55ca2..e20659233e26c021129f4e72253632e726a08794 100644
--- a/include/core/utils/utils_gtest.h
+++ b/include/core/utils/utils_gtest.h
@@ -113,12 +113,12 @@ TEST(Test, Foo)
/* Macros related to testing Eigen classes:
*/
-#define EXPECT_MATRIX_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
+#define EXPECT_MATRIX_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
return (lhs - rhs).isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
-#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
+#define ASSERT_MATRIX_APPROX(C_expect, C_actual, precision) ASSERT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
return (lhs - rhs).isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
@@ -127,15 +127,15 @@ TEST(Test, Foo)
#define ASSERT_QUATERNION_APPROX(C_expect, C_actual, precision) ASSERT_MATRIX_APPROX((C_expect).coeffs(), (C_actual).coeffs(), precision)
-#define EXPECT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
- MatrixXs er = lhs - rhs; \
+#define EXPECT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+ MatrixXd er = lhs - rhs; \
er(2) = pi2pi((double)er(2)); \
return er.isMuchSmallerThan(1, precision); \
}, \
C_expect, C_actual);
-#define ASSERT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXs lhs, const Eigen::MatrixXs rhs) { \
- MatrixXs er = lhs - rhs; \
+#define ASSERT_POSE2D_APPROX(C_expect, C_actual, precision) EXPECT_PRED2([](const Eigen::MatrixXd lhs, const Eigen::MatrixXd rhs) { \
+ MatrixXd er = lhs - rhs; \
er(2) = pi2pi((double)er(2)); \
return er.isMuchSmallerThan(1, precision); \
}, \
diff --git a/src/capture/capture_base.cpp b/src/capture/capture_base.cpp
index 1001062fcb522bf500dd89985a33669ee08c59ee..cee1037ba529d7d89a06747471e51a313778f9ae 100644
--- a/src/capture/capture_base.cpp
+++ b/src/capture/capture_base.cpp
@@ -257,9 +257,9 @@ SizeEigen CaptureBase::computeCalibSize() const
return sz;
}
-Eigen::VectorXs CaptureBase::getCalibration() const
+Eigen::VectorXd CaptureBase::getCalibration() const
{
- Eigen::VectorXs calib(calib_size_);
+ Eigen::VectorXd calib(calib_size_);
SizeEigen index = 0;
for (unsigned int i = 0; i < getStateBlockVec().size(); i++)
{
@@ -273,7 +273,7 @@ Eigen::VectorXs CaptureBase::getCalibration() const
return calib;
}
-void CaptureBase::setCalibration(const VectorXs& _calib)
+void CaptureBase::setCalibration(const VectorXd& _calib)
{
updateCalibSize();
assert(_calib.size() == calib_size_ && "Wrong size of calibration vector");
diff --git a/src/capture/capture_diff_drive.cpp b/src/capture/capture_diff_drive.cpp
index cc81850c8447040d21f7e6cffa4f079c5884ca47..b774104e64e76bac44edc4a3bd5cb537661c1dfa 100644
--- a/src/capture/capture_diff_drive.cpp
+++ b/src/capture/capture_diff_drive.cpp
@@ -7,8 +7,8 @@ namespace wolf {
CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr,
@@ -28,10 +28,10 @@ CaptureDiffDrive::CaptureDiffDrive(const TimeStamp& _ts,
//
}
-Eigen::VectorXs CaptureDiffDrive::correctDelta(const VectorXs& _delta,
- const VectorXs& _delta_error) const
+Eigen::VectorXd CaptureDiffDrive::correctDelta(const VectorXd& _delta,
+ const VectorXd& _delta_error) const
{
- Vector3s delta_corrected = _delta + _delta_error;
+ Vector3d delta_corrected = _delta + _delta_error;
delta_corrected(2) = pi2pi(delta_corrected(2));
return delta_corrected;
}
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index a2330905ad8be9ff5f871f8e5fc0c11a01b9ccde..a736c242e7fb7047498d061d9121ce8fb56f817c 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -13,13 +13,13 @@ namespace wolf
CaptureMotion::CaptureMotion(const std::string & _type,
const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
+ const Eigen::VectorXd& _data,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr) :
CaptureBase(_type, _ts, _sensor_ptr),
data_(_data),
- data_cov_(_sensor_ptr ? _sensor_ptr->getNoiseCov() : Eigen::MatrixXs::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
+ data_cov_(_sensor_ptr ? _sensor_ptr->getNoiseCov() : Eigen::MatrixXd::Zero(_data.rows(), _data.rows())), // Someone should test this zero and do something smart accordingly
buffer_(),
capture_origin_ptr_(_capture_origin_ptr)
{
@@ -29,8 +29,8 @@ CaptureMotion::CaptureMotion(const std::string & _type,
CaptureMotion::CaptureMotion(const std::string & _type,
const TimeStamp& _ts,
SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
SizeEigen _delta_size,
SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr,
@@ -51,23 +51,23 @@ CaptureMotion::~CaptureMotion()
//
}
-Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current) const
+Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current) const
{
- VectorXs calib_preint = getCalibrationPreint();
- VectorXs delta_preint = getBuffer().get().back().delta_integr_;
- MatrixXs jac_calib = getBuffer().get().back().jacobian_calib_;
- VectorXs delta_error = jac_calib * (_calib_current - calib_preint);
- VectorXs delta_corrected = correctDelta(delta_preint, delta_error);
+ VectorXd calib_preint = getCalibrationPreint();
+ VectorXd delta_preint = getBuffer().get().back().delta_integr_;
+ MatrixXd jac_calib = getBuffer().get().back().jacobian_calib_;
+ VectorXd delta_error = jac_calib * (_calib_current - calib_preint);
+ VectorXd delta_corrected = correctDelta(delta_preint, delta_error);
return delta_corrected;
}
-Eigen::VectorXs CaptureMotion::getDeltaCorrected(const VectorXs& _calib_current, const TimeStamp& _ts) const
+Eigen::VectorXd CaptureMotion::getDeltaCorrected(const VectorXd& _calib_current, const TimeStamp& _ts) const
{
Motion motion = getBuffer().getMotion(_ts);
- VectorXs delta_preint = motion.delta_integr_;
- MatrixXs jac_calib = motion.jacobian_calib_;
- VectorXs delta_error = jac_calib * (_calib_current - calib_preint_);
- VectorXs delta_corrected = correctDelta(delta_preint, delta_error);
+ VectorXd delta_preint = motion.delta_integr_;
+ MatrixXd jac_calib = motion.jacobian_calib_;
+ VectorXd delta_error = jac_calib * (_calib_current - calib_preint_);
+ VectorXd delta_corrected = correctDelta(delta_preint, delta_error);
return delta_corrected;
}
diff --git a/src/capture/capture_odom_2D.cpp b/src/capture/capture_odom_2D.cpp
index 36826540a27804e14c61102260bbec24a36df58f..003380c63bfca6dc865b728e9a59635e87232108 100644
--- a/src/capture/capture_odom_2D.cpp
+++ b/src/capture/capture_odom_2D.cpp
@@ -12,7 +12,7 @@ namespace wolf
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
+ const Eigen::VectorXd& _data,
CaptureBasePtr _capture_origin_ptr):
CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, 3, 3, _capture_origin_ptr)
{
@@ -21,8 +21,8 @@ CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts,
CaptureOdom2D::CaptureOdom2D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr):
CaptureMotion("ODOM 2D", _init_ts, _sensor_ptr, _data, _data_cov, 3, 3, _capture_origin_ptr)
{
diff --git a/src/capture/capture_odom_3D.cpp b/src/capture/capture_odom_3D.cpp
index 84f263d2cfbd6c97e647b8504322352696d4332d..65223b947e15056ede8d81f85de7d81216fec7f3 100644
--- a/src/capture/capture_odom_3D.cpp
+++ b/src/capture/capture_odom_3D.cpp
@@ -12,7 +12,7 @@ namespace wolf
CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
+ const Eigen::VectorXd& _data,
CaptureBasePtr _capture_origin_ptr):
CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, 7, 6, _capture_origin_ptr)
{
@@ -21,8 +21,8 @@ CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
CaptureOdom3D::CaptureOdom3D(const TimeStamp& _init_ts,
SensorBasePtr _sensor_ptr,
- const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
+ const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
CaptureBasePtr _capture_origin_ptr):
CaptureMotion("ODOM 3D", _init_ts, _sensor_ptr, _data, _data_cov, 7, 6, _capture_origin_ptr)
{
@@ -34,11 +34,11 @@ CaptureOdom3D::~CaptureOdom3D()
//
}
-Eigen::VectorXs CaptureOdom3D::correctDelta(const VectorXs& _delta, const VectorXs& _delta_error) const
+Eigen::VectorXd CaptureOdom3D::correctDelta(const VectorXd& _delta, const VectorXd& _delta_error) const
{
- VectorXs delta(7);
+ VectorXd delta(7);
delta.head(3) = _delta.head(3) + _delta_error.head(3);
- delta.tail(4) = (Quaternions(_delta.data()+3) * exp_q(_delta_error.tail(3))).coeffs();
+ delta.tail(4) = (Quaterniond(_delta.data()+3) * exp_q(_delta_error.tail(3))).coeffs();
return delta;
}
diff --git a/src/capture/capture_pose.cpp b/src/capture/capture_pose.cpp
index 46177687a1cbaab1a507ba3ac4e091aec7d9b7fd..a86ad2cd09cad733fab74326a983d01c3a529db7 100644
--- a/src/capture/capture_pose.cpp
+++ b/src/capture/capture_pose.cpp
@@ -2,7 +2,7 @@
namespace wolf{
-CapturePose::CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
+CapturePose::CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_covariance) :
CaptureBase("POSE", _ts, _sensor_ptr),
data_(_data),
data_covariance_(_data_covariance)
diff --git a/src/capture/capture_velocity.cpp b/src/capture/capture_velocity.cpp
index 00a6b796bbba9135ee8fbba1150ff353c29b72ec..fb78b8f0c32deae0f6556edcf47db99459bd3e20 100644
--- a/src/capture/capture_velocity.cpp
+++ b/src/capture/capture_velocity.cpp
@@ -4,7 +4,7 @@ namespace wolf {
CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _velocity,
+ const Eigen::VectorXd& _velocity,
SizeEigen _delta_size, SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr) :
CaptureMotion("VELOCITY", _ts, _sensor_ptr, _velocity,
@@ -15,8 +15,8 @@ CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
const SensorBasePtr& _sensor_ptr,
- const Eigen::VectorXs& _velocity,
- const Eigen::MatrixXs& _velocity_cov,
+ const Eigen::VectorXd& _velocity,
+ const Eigen::MatrixXd& _velocity_cov,
SizeEigen _delta_size, SizeEigen _delta_cov_size,
CaptureBasePtr _capture_origin_ptr,
StateBlockPtr _p_ptr,
@@ -29,12 +29,12 @@ CaptureVelocity::CaptureVelocity(const TimeStamp& _ts,
//
}
-const Eigen::VectorXs& CaptureVelocity::getVelocity() const
+const Eigen::VectorXd& CaptureVelocity::getVelocity() const
{
return getData();
}
-const Eigen::MatrixXs& CaptureVelocity::getVelocityCov() const
+const Eigen::MatrixXd& CaptureVelocity::getVelocityCov() const
{
return getDataCovariance();
}
diff --git a/src/ceres_wrapper/local_parametrization_wrapper.cpp b/src/ceres_wrapper/local_parametrization_wrapper.cpp
index 004938658ca78c7c233a96dfcd45825a65c4cda3..4607161970aa0c67928c5587cfc8a42db66407d0 100644
--- a/src/ceres_wrapper/local_parametrization_wrapper.cpp
+++ b/src/ceres_wrapper/local_parametrization_wrapper.cpp
@@ -4,16 +4,16 @@ namespace wolf {
bool LocalParametrizationWrapper::Plus(const double* x_raw, const double* delta_raw, double* x_plus_delta_raw) const
{
- Eigen::Map<const Eigen::VectorXs> x_raw_map((double*)x_raw, GlobalSize());
- Eigen::Map<const Eigen::VectorXs> delta_raw_map((double*)delta_raw, LocalSize());
- Eigen::Map<Eigen::VectorXs> x_plus_map((double*)x_plus_delta_raw, GlobalSize());
+ Eigen::Map<const Eigen::VectorXd> x_raw_map((double*)x_raw, GlobalSize());
+ Eigen::Map<const Eigen::VectorXd> delta_raw_map((double*)delta_raw, LocalSize());
+ Eigen::Map<Eigen::VectorXd> x_plus_map((double*)x_plus_delta_raw, GlobalSize());
return local_parametrization_ptr_->plus(x_raw_map, delta_raw_map, x_plus_map);
};
bool LocalParametrizationWrapper::ComputeJacobian(const double* x, double* jacobian) const
{
- Eigen::Map<const Eigen::VectorXs> x_map((double*)x, GlobalSize());
- Eigen::Map<Eigen::MatrixXs> jacobian_map((double*)jacobian, GlobalSize(), LocalSize());
+ Eigen::Map<const Eigen::VectorXd> x_map((double*)x, GlobalSize());
+ Eigen::Map<Eigen::MatrixXd> jacobian_map((double*)jacobian, GlobalSize(), LocalSize());
return local_parametrization_ptr_->computeJacobian(x_map, jacobian_map);
};
diff --git a/src/ceres_wrapper/qr_manager.cpp b/src/ceres_wrapper/qr_manager.cpp
index 61355878340b4eb68e79fb7e5305cdd63507f5e3..0d7746f929f427d3e2bc9f6bcce53e4147b7924d 100644
--- a/src/ceres_wrapper/qr_manager.cpp
+++ b/src/ceres_wrapper/qr_manager.cpp
@@ -37,7 +37,7 @@ std::string QRManager::solve(const unsigned int& _report_level)
return std::string("decomposition failed\n");
// Solve
- Eigen::VectorXs x_incr = solver_.solve(-b_);
+ Eigen::VectorXd x_incr = solver_.solve(-b_);
b_.setZero();
// update state blocks
@@ -64,18 +64,18 @@ void QRManager::computeCovariances(const StateBlockPtrList& _sb_list)
computeDecomposition();
// std::cout << "R is " << solver_.matrixR().rows() << "x" << solver_.matrixR().cols() << std::endl;
-// std::cout << Eigen::MatrixXs(solver_.matrixR()) << std::endl;
+// std::cout << Eigen::MatrixXd(solver_.matrixR()) << std::endl;
covariance_solver_.compute(solver_.matrixR().topRows(solver_.matrixR().cols()));
Eigen::SparseMatrix<double, Eigen::ColMajor> I(A_.cols(),A_.cols());
I.setIdentity();
Eigen::SparseMatrix<double, Eigen::ColMajor> iR = covariance_solver_.solve(I);
- Eigen::MatrixXs Sigma_full = solver_.colsPermutation() * iR * iR.transpose() * solver_.colsPermutation().transpose();
+ Eigen::MatrixXd Sigma_full = solver_.colsPermutation() * iR * iR.transpose() * solver_.colsPermutation().transpose();
-// std::cout << "A' A = \n" << Eigen::MatrixXs(A_.transpose() * A_)<< std::endl;
-// std::cout << "P iR iR' P' = \n" << Eigen::MatrixXs(P * iR * iR.transpose() * P.transpose()) << std::endl;
-// std::cout << "Sigma * Lambda = \n" << Eigen::MatrixXs(Sigma_full * A_.transpose() * A_) << std::endl;
+// std::cout << "A' A = \n" << Eigen::MatrixXd(A_.transpose() * A_)<< std::endl;
+// std::cout << "P iR iR' P' = \n" << Eigen::MatrixXd(P * iR * iR.transpose() * P.transpose()) << std::endl;
+// std::cout << "Sigma * Lambda = \n" << Eigen::MatrixXd(Sigma_full * A_.transpose() * A_) << std::endl;
// std::cout << "Permutation: \n" << solver_.colsPermutation() << std::endl;
// std::cout << "Sigma = \n" << Sigma_full << std::endl;
@@ -208,12 +208,12 @@ void QRManager::relinearizeFactor(FactorBasePtr _fac_ptr)
std::vector<const double*> fac_states_ptr;
for (auto sb : _fac_ptr->getStateBlockPtrVector())
fac_states_ptr.push_back(sb->get());
- Eigen::VectorXs residual(_fac_ptr->getSize());
- std::vector<Eigen::MatrixXs> jacobians;
+ Eigen::VectorXd residual(_fac_ptr->getSize());
+ std::vector<Eigen::MatrixXd> jacobians;
_fac_ptr->evaluate(fac_states_ptr,residual,jacobians);
// Fill jacobians
- Eigen::SparseMatrixs A_block_row(_fac_ptr->getSize(), A_.cols());
+ Eigen::SparseMatrixd A_block_row(_fac_ptr->getSize(), A_.cols());
for (auto i = 0; i < jacobians.size(); i++)
if (!_fac_ptr->getStateBlockPtrVector()[i]->isFixed())
{
diff --git a/src/factor/factor_base.cpp b/src/factor/factor_base.cpp
index f64a1464b0c441f43c731b1a4420cddc3d3794fd..d501b32fba6773848d94a50a6ae5524ff2632661 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -98,19 +98,19 @@ void FactorBase::remove(bool viral_remove_empty_parent)
}
}
-const Eigen::VectorXs& FactorBase::getMeasurement() const
+const Eigen::VectorXd& FactorBase::getMeasurement() const
{
assert(getFeature() != nullptr && "calling getMeasurement before linking with a feature");
return getFeature()->getMeasurement();
}
-const Eigen::MatrixXs& FactorBase::getMeasurementCovariance() const
+const Eigen::MatrixXd& FactorBase::getMeasurementCovariance() const
{
assert(getFeature() != nullptr && "calling getMeasurementCovariance before linking with a feature");
return getFeature()->getMeasurementCovariance();
}
-const Eigen::MatrixXs& FactorBase::getMeasurementSquareRootInformationUpper() const
+const Eigen::MatrixXd& FactorBase::getMeasurementSquareRootInformationUpper() const
{
assert(getFeature() != nullptr && "calling getMeasurementSquareRootInformationUpper before linking with a feature");
return getFeature()->getMeasurementSquareRootInformationUpper();
diff --git a/src/feature/feature_base.cpp b/src/feature/feature_base.cpp
index 2f6ceb15bdd8a478a8cff81ef1fb9ace00db3e0f..caeba3c2e91d0b641ee8bd7ffd7e1e951828198c 100644
--- a/src/feature/feature_base.cpp
+++ b/src/feature/feature_base.cpp
@@ -6,7 +6,7 @@ namespace wolf {
unsigned int FeatureBase::feature_id_count_ = 0;
-FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_uncertainty, UncertaintyType _uncertainty_type) :
+FeatureBase::FeatureBase(const std::string& _type, const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_uncertainty, UncertaintyType _uncertainty_type) :
NodeBase("FEATURE", _type),
capture_ptr_(),
feature_id_(++feature_id_count_),
@@ -101,7 +101,7 @@ void FeatureBase::getFactorList(FactorBasePtrList & _fac_list) const
_fac_list.insert(_fac_list.end(), factor_list_.begin(), factor_list_.end());
}
-void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
+void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXd & _meas_cov)
{
WOLF_ASSERT_COVARIANCE_MATRIX(_meas_cov);
@@ -112,7 +112,7 @@ void FeatureBase::setMeasurementCovariance(const Eigen::MatrixXs & _meas_cov)
measurement_sqrt_information_upper_ = computeSqrtUpper(measurement_covariance_.inverse());
}
-void FeatureBase::setMeasurementInformation(const Eigen::MatrixXs & _meas_info)
+void FeatureBase::setMeasurementInformation(const Eigen::MatrixXd & _meas_info)
{
WOLF_ASSERT_INFORMATION_MATRIX(_meas_info);
@@ -133,22 +133,22 @@ void FeatureBase::setProblem(ProblemPtr _problem)
fac->setProblem(_problem);
}
-Eigen::MatrixXs FeatureBase::computeSqrtUpper(const Eigen::MatrixXs & _info) const
+Eigen::MatrixXd FeatureBase::computeSqrtUpper(const Eigen::MatrixXd & _info) const
{
// impose symmetry
- Eigen::MatrixXs info = _info.selfadjointView<Eigen::Upper>();
+ Eigen::MatrixXd info = _info.selfadjointView<Eigen::Upper>();
// Normal Cholesky factorization
- Eigen::LLT<Eigen::MatrixXs> llt_of_info(info);
- Eigen::MatrixXs R = llt_of_info.matrixU();
+ Eigen::LLT<Eigen::MatrixXd> llt_of_info(info);
+ Eigen::MatrixXd R = llt_of_info.matrixU();
// Good factorization
if (info.isApprox(R.transpose() * R, Constants::EPS))
return R;
// Not good factorization: SelfAdjointEigenSolver
- Eigen::SelfAdjointEigenSolver<Eigen::MatrixXs> es(info);
- Eigen::VectorXs eval = es.eigenvalues().real().cwiseMax(Constants::EPS);
+ Eigen::SelfAdjointEigenSolver<Eigen::MatrixXd> es(info);
+ Eigen::VectorXd eval = es.eigenvalues().real().cwiseMax(Constants::EPS);
R = eval.cwiseSqrt().asDiagonal() * es.eigenvectors().real().transpose();
diff --git a/src/feature/feature_diff_drive.cpp b/src/feature/feature_diff_drive.cpp
index ecf92ab78148361f036d7df23cae237a998a98d4..5013ada7f3607bf903487cf2749a5c00df67915f 100644
--- a/src/feature/feature_diff_drive.cpp
+++ b/src/feature/feature_diff_drive.cpp
@@ -3,10 +3,10 @@
namespace wolf
{
-FeatureDiffDrive::FeatureDiffDrive(const Eigen::VectorXs& _delta_preintegrated,
- const Eigen::MatrixXs& _delta_preintegrated_covariance,
- const Eigen::VectorXs& _diff_drive_params,
- const Eigen::MatrixXs& _jacobian_diff_drive_params) :
+FeatureDiffDrive::FeatureDiffDrive(const Eigen::VectorXd& _delta_preintegrated,
+ const Eigen::MatrixXd& _delta_preintegrated_covariance,
+ const Eigen::VectorXd& _diff_drive_params,
+ const Eigen::MatrixXd& _jacobian_diff_drive_params) :
FeatureMotion("DIFF DRIVE",
_delta_preintegrated,
_delta_preintegrated_covariance,
diff --git a/src/feature/feature_motion.cpp b/src/feature/feature_motion.cpp
index 078776584865b462f67e355fcfb30c643ad1578f..d0b988b79efd5b8e376c6c244a7154933479538f 100644
--- a/src/feature/feature_motion.cpp
+++ b/src/feature/feature_motion.cpp
@@ -11,10 +11,10 @@ namespace wolf
{
FeatureMotion::FeatureMotion(const std::string& _type,
- const VectorXs& _delta_preint,
- const MatrixXs _delta_preint_cov,
- const VectorXs& _calib_preint,
- const MatrixXs& _jacobian_calib) :
+ const VectorXd& _delta_preint,
+ const MatrixXd _delta_preint_cov,
+ const VectorXd& _calib_preint,
+ const MatrixXd& _jacobian_calib) :
FeatureBase(_type, _delta_preint, _delta_preint_cov),
calib_preint_(_calib_preint),
jacobian_calib_(_jacobian_calib)
diff --git a/src/feature/feature_odom_2D.cpp b/src/feature/feature_odom_2D.cpp
index ee6cd60ba38a16a161925d79512eaf9fa1e6212d..6227d1b784f5e826cbf373061eda4d5131624b22 100644
--- a/src/feature/feature_odom_2D.cpp
+++ b/src/feature/feature_odom_2D.cpp
@@ -2,7 +2,7 @@
namespace wolf {
-FeatureOdom2D::FeatureOdom2D(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
+FeatureOdom2D::FeatureOdom2D(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
FeatureBase("ODOM 2D", _measurement, _meas_covariance)
{
//std::cout << "New FeatureOdom2D: measurement " << _measurement.transpose() << std::endl << "covariance" << std::endl << _meas_covariance << std::endl;
diff --git a/src/feature/feature_pose.cpp b/src/feature/feature_pose.cpp
index 344d230a7164d0c222567ae9ea7e1000be23cb84..e918d049b23f537f6fcde0b0108ca64c0ff5e368 100644
--- a/src/feature/feature_pose.cpp
+++ b/src/feature/feature_pose.cpp
@@ -2,7 +2,7 @@
namespace wolf {
-FeaturePose::FeaturePose(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
+FeaturePose::FeaturePose(const Eigen::VectorXd& _measurement, const Eigen::MatrixXd& _meas_covariance) :
FeatureBase("POSE", _measurement, _meas_covariance)
{
//
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index be3615dc02cf17d3c93ebc9d259c32e85aa0d8c3..3807a7d13fab7e8a3ac99c881a16498dabe04a46 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -37,7 +37,7 @@ FrameBase::FrameBase(const FrameType & _tp, const TimeStamp& _ts, StateBlockPtr
state_block_vec_[2] = _v_ptr;
}
-FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXs& _x) :
+FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, const FrameType & _tp, const TimeStamp& _ts, const Eigen::VectorXd& _x) :
NodeBase("FRAME", "Base"),
trajectory_ptr_(),
state_block_vec_(3), // allow for 3 state blocks by default. Resize in derived constructors if needed.
@@ -46,7 +46,7 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
time_stamp_(_ts)
{
if(_frame_structure.compare("PO") == 0 and _dim == 2){
- // auto _x = Eigen::VectorXs::Zero(3);
+ // auto _x = Eigen::VectorXd::Zero(3);
assert(_x.size() == 3 && "Wrong state vector size. Should be 3 for 2D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <2> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateAngle> ((double) _x(2) ) );
@@ -56,7 +56,7 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
state_block_vec_[2] = v_ptr;
this->setType("PO 2D");
}else if(_frame_structure.compare("PO") == 0 and _dim == 3){
- // auto _x = Eigen::VectorXs::Zero(7);
+ // auto _x = Eigen::VectorXd::Zero(7);
assert(_x.size() == 7 && "Wrong state vector size. Should be 7 for 3D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.tail <4> ( ) ) );
@@ -66,7 +66,7 @@ FrameBase::FrameBase(const std::string _frame_structure, const SizeEigen _dim, c
state_block_vec_[2] = v_ptr;
this->setType("PO 3D");
}else if(_frame_structure.compare("POV") == 0 and _dim == 3){
- // auto _x = Eigen::VectorXs::Zero(10);
+ // auto _x = Eigen::VectorXd::Zero(10);
assert(_x.size() == 10 && "Wrong state vector size. Should be 10 for 3D!");
StateBlockPtr p_ptr ( std::make_shared<StateBlock> (_x.head <3> ( ) ) );
StateBlockPtr o_ptr ( std::make_shared<StateQuaternion> (_x.segment <4> (3) ) );
@@ -219,7 +219,7 @@ bool FrameBase::isFixed() const
return fixed;
}
-void FrameBase::setState(const Eigen::VectorXs& _state)
+void FrameBase::setState(const Eigen::VectorXd& _state)
{
int size = 0;
for(unsigned int i = 0; i<state_block_vec_.size(); i++){
@@ -242,23 +242,23 @@ void FrameBase::setState(const Eigen::VectorXs& _state)
}
}
-Eigen::VectorXs FrameBase::getState() const
+Eigen::VectorXd FrameBase::getState() const
{
- Eigen::VectorXs state;
+ Eigen::VectorXd state;
getState(state);
return state;
}
-void FrameBase::getState(Eigen::VectorXs& _state) const
+void FrameBase::getState(Eigen::VectorXd& _state) const
{
SizeEigen size = 0;
for (StateBlockPtr sb : state_block_vec_)
if (sb)
size += sb->getSize();
- _state = Eigen::VectorXs(size);
+ _state = Eigen::VectorXd(size);
SizeEigen index = 0;
for (StateBlockPtr sb : state_block_vec_)
@@ -278,7 +278,7 @@ unsigned int FrameBase::getSize() const
return size;
}
-bool FrameBase::getCovariance(Eigen::MatrixXs& _cov) const
+bool FrameBase::getCovariance(Eigen::MatrixXd& _cov) const
{
return getProblem()->getFrameCovariance(shared_from_this(), _cov);
}
diff --git a/src/landmark/landmark_base.cpp b/src/landmark/landmark_base.cpp
index 20e32da0a4e7d5d03a4475cb7ed37e54a7865311..283e8f19cf1869eefb5d0c6c03855ee126b882f1 100644
--- a/src/landmark/landmark_base.cpp
+++ b/src/landmark/landmark_base.cpp
@@ -98,7 +98,7 @@ void LandmarkBase::registerNewStateBlocks()
}
}
-bool LandmarkBase::getCovariance(Eigen::MatrixXs& _cov) const
+bool LandmarkBase::getCovariance(Eigen::MatrixXd& _cov) const
{
return getProblem()->getLandmarkCovariance(shared_from_this(), _cov);
}
@@ -119,23 +119,23 @@ void LandmarkBase::removeStateBlocks()
}
}
-Eigen::VectorXs LandmarkBase::getState() const
+Eigen::VectorXd LandmarkBase::getState() const
{
- Eigen::VectorXs state;
+ Eigen::VectorXd state;
getState(state);
return state;
}
-void LandmarkBase::getState(Eigen::VectorXs& _state) const
+void LandmarkBase::getState(Eigen::VectorXd& _state) const
{
SizeEigen size = 0;
for (StateBlockPtr sb : state_block_vec_)
if (sb)
size += sb->getSize();
- _state = Eigen::VectorXs(size);
+ _state = Eigen::VectorXd(size);
SizeEigen index = 0;
for (StateBlockPtr sb : state_block_vec_)
@@ -204,7 +204,7 @@ void LandmarkBase::removeConstrainedBy(FactorBasePtr _fac_ptr)
LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
{
unsigned int id = _node["id"] .as< unsigned int >();
- Eigen::VectorXs pos = _node["position"] .as< Eigen::VectorXs >();
+ Eigen::VectorXd pos = _node["position"] .as< Eigen::VectorXd >();
bool pos_fixed = _node["position fixed"] .as< bool >();
StateBlockPtr pos_sb = std::make_shared<StateBlock>(pos, pos_fixed);
@@ -212,7 +212,7 @@ LandmarkBasePtr LandmarkBase::create(const YAML::Node& _node)
if (_node["orientation"])
{
- Eigen::VectorXs ori = _node["orientation"].as< Eigen::VectorXs >();
+ Eigen::VectorXd ori = _node["orientation"].as< Eigen::VectorXd >();
bool ori_fixed = _node["orientation fixed"].as< bool >();
if (ori.size() == 4)
diff --git a/src/problem/problem.cpp b/src/problem/problem.cpp
index 7306aa33a60b91306f2d1934a2ce47984b5ce2f6..40ce05fc02bdd6620d283c0e433fc9421d3d3609 100644
--- a/src/problem/problem.cpp
+++ b/src/problem/problem.cpp
@@ -155,8 +155,8 @@ ProblemPtr Problem::autoSetup(ParamsServer &_server)
}
// Prior
- Eigen::VectorXs prior_state = _server.getParam<Eigen::VectorXs>("problem/prior/state");
- Eigen::MatrixXs prior_cov = _server.getParam<Eigen::MatrixXs>("problem/prior/cov");
+ Eigen::VectorXd prior_state = _server.getParam<Eigen::VectorXd>("problem/prior/state");
+ Eigen::MatrixXd prior_cov = _server.getParam<Eigen::MatrixXd>("problem/prior/cov");
double prior_time_tolerance = _server.getParam<double>("problem/prior/time_tolerance");
double prior_ts = _server.getParam<double>("problem/prior/timestamp");
@@ -178,7 +178,7 @@ Problem::~Problem()
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
- const Eigen::VectorXs& _extrinsics, //
+ const Eigen::VectorXd& _extrinsics, //
IntrinsicsBasePtr _intrinsics)
{
SensorBasePtr sen_ptr = SensorFactory::get().create(uppercase(_sen_type), _unique_sensor_name, _extrinsics, _intrinsics);
@@ -188,7 +188,7 @@ SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
SensorBasePtr Problem::installSensor(const std::string& _sen_type, //
const std::string& _unique_sensor_name, //
- const Eigen::VectorXs& _extrinsics, //
+ const Eigen::VectorXd& _extrinsics, //
const std::string& _intrinsics_filename)
{
@@ -340,7 +340,7 @@ void Problem::clearProcessorMotion()
FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
const SizeEigen _dim, //
FrameType _frame_key_type, //
- const Eigen::VectorXs& _frame_state, //
+ const Eigen::VectorXd& _frame_state, //
const TimeStamp& _time_stamp)
{
auto frm = FrameBase::emplace<FrameBase>(trajectory_ptr_, _frame_structure, _dim, _frame_key_type, _time_stamp, _frame_state);
@@ -356,7 +356,7 @@ FrameBasePtr Problem::emplaceFrame(const std::string& _frame_structure, //
}
FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
- const Eigen::VectorXs& _frame_state, //
+ const Eigen::VectorXd& _frame_state, //
const TimeStamp& _time_stamp)
{
return emplaceFrame(this->getFrameStructure(), this->getDim(), _frame_key_type, _frame_state, _time_stamp);
@@ -368,14 +368,14 @@ FrameBasePtr Problem::emplaceFrame(FrameType _frame_key_type, //
return emplaceFrame(this->getFrameStructure(), this->getDim(), _frame_key_type, _time_stamp);
}
-Eigen::VectorXs Problem::getCurrentState() const
+Eigen::VectorXd Problem::getCurrentState() const
{
- Eigen::VectorXs state(getFrameStructureSize());
+ Eigen::VectorXd state(getFrameStructureSize());
getCurrentState(state);
return state;
}
-void Problem::getCurrentState(Eigen::VectorXs& _state) const
+void Problem::getCurrentState(Eigen::VectorXd& _state) const
{
if (processor_motion_ptr_ != nullptr)
processor_motion_ptr_->getCurrentState(_state);
@@ -385,7 +385,7 @@ void Problem::getCurrentState(Eigen::VectorXs& _state) const
_state = zeroState();
}
-void Problem::getCurrentStateAndStamp(Eigen::VectorXs& _state, TimeStamp& ts) const
+void Problem::getCurrentStateAndStamp(Eigen::VectorXd& _state, TimeStamp& ts) const
{
if (processor_motion_ptr_ != nullptr)
{
@@ -401,7 +401,7 @@ void Problem::getCurrentStateAndStamp(Eigen::VectorXs& _state, TimeStamp& ts) co
_state = zeroState();
}
-void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& _state) const
+void Problem::getState(const TimeStamp& _ts, Eigen::VectorXd& _state) const
{
// try to get the state from processor_motion if any, otherwise...
if (processor_motion_ptr_ == nullptr || !processor_motion_ptr_->getState(_ts, _state))
@@ -414,9 +414,9 @@ void Problem::getState(const TimeStamp& _ts, Eigen::VectorXs& _state) const
}
}
-Eigen::VectorXs Problem::getState(const TimeStamp& _ts) const
+Eigen::VectorXd Problem::getState(const TimeStamp& _ts) const
{
- Eigen::VectorXs state(getFrameStructureSize());
+ Eigen::VectorXd state(getFrameStructureSize());
getState(_ts, state);
return state;
}
@@ -441,9 +441,9 @@ std::string Problem::getFrameStructure() const
return frame_structure_;
}
-Eigen::VectorXs Problem::zeroState() const
+Eigen::VectorXd Problem::zeroState() const
{
- Eigen::VectorXs state = Eigen::VectorXs::Zero(getFrameStructureSize());
+ Eigen::VectorXd state = Eigen::VectorXd::Zero(getFrameStructureSize());
// Set the quaternion identity for 3D states. Set only the real part to 1:
if(this->getDim() == 3)
@@ -575,7 +575,7 @@ void Problem::clearCovariance()
covariances_.clear();
}
-void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXs& _cov)
+void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, const Eigen::MatrixXd& _cov)
{
assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
assert(_state2->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
@@ -584,7 +584,7 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, c
covariances_[std::pair<StateBlockPtr, StateBlockPtr>(_state1, _state2)] = _cov;
}
-void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _cov)
+void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXd& _cov)
{
assert(_state1->getLocalSize() == (unsigned int ) _cov.rows() && "wrong covariance block size");
assert(_state1->getLocalSize() == (unsigned int ) _cov.cols() && "wrong covariance block size");
@@ -593,7 +593,7 @@ void Problem::addCovarianceBlock(StateBlockPtr _state1, const Eigen::MatrixXs& _
covariances_[std::make_pair(_state1, _state1)] = _cov;
}
-bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXs& _cov, const int _row,
+bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, Eigen::MatrixXd& _cov, const int _row,
const int _col) const
{
//std::cout << "entire cov to be filled:" << std::endl << _cov << std::endl;
@@ -626,7 +626,7 @@ bool Problem::getCovarianceBlock(StateBlockPtr _state1, StateBlockPtr _state2, E
return true;
}
-bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXs& _cov) const
+bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx, Eigen::MatrixXd& _cov) const
{
std::lock_guard<std::mutex> lock(mut_covariances_);
@@ -667,12 +667,12 @@ bool Problem::getCovarianceBlock(std::map<StateBlockPtr, unsigned int> _sb_2_idx
return true;
}
-bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXs& _cov, const int _row_and_col) const
+bool Problem::getCovarianceBlock(StateBlockPtr _state, Eigen::MatrixXd& _cov, const int _row_and_col) const
{
return getCovarianceBlock(_state, _state, _cov, _row_and_col, _row_and_col);
}
-bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs& _covariance) const
+bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXd& _covariance) const
{
bool success(true);
int i = 0, j = 0;
@@ -686,7 +686,7 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
sz += sb->getLocalSize();
// resizing
- _covariance = Eigen::MatrixXs(sz, sz);
+ _covariance = Eigen::MatrixXd(sz, sz);
// filling covariance
for (const auto& sb_i : state_bloc_vec)
@@ -708,19 +708,19 @@ bool Problem::getFrameCovariance(FrameBaseConstPtr _frame_ptr, Eigen::MatrixXs&
return success;
}
-bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXs& cov) const
+bool Problem::getLastKeyFrameCovariance(Eigen::MatrixXd& cov) const
{
FrameBasePtr frm = getLastKeyFrame();
return getFrameCovariance(frm, cov);
}
-bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXs& cov) const
+bool Problem::getLastKeyOrAuxFrameCovariance(Eigen::MatrixXd& cov) const
{
FrameBasePtr frm = getLastKeyOrAuxFrame();
return getFrameCovariance(frm, cov);
}
-bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXs& _covariance) const
+bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::MatrixXd& _covariance) const
{
bool success(true);
int i = 0, j = 0;
@@ -734,7 +734,7 @@ bool Problem::getLandmarkCovariance(LandmarkBaseConstPtr _landmark_ptr, Eigen::M
sz += sb->getLocalSize();
// resizing
- _covariance = Eigen::MatrixXs(sz, sz);
+ _covariance = Eigen::MatrixXd(sz, sz);
// filling covariance
@@ -797,7 +797,7 @@ FrameBasePtr Problem::closestKeyOrAuxFrameToTimeStamp(const TimeStamp& _ts) cons
return trajectory_ptr_->closestKeyOrAuxFrameToTimeStamp(_ts);
}
-FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen::MatrixXs& _prior_cov, const TimeStamp& _ts, const double _time_tolerance)
+FrameBasePtr Problem::setPrior(const Eigen::VectorXd& _prior_state, const Eigen::MatrixXd& _prior_cov, const TimeStamp& _ts, const double _time_tolerance)
{
if ( ! prior_is_set_ )
{
diff --git a/src/processor/motion_buffer.cpp b/src/processor/motion_buffer.cpp
index 1a4aa63e0179e4bf859c890246b70d04c2e9fb27..5d0a595673da23a73efd69d6b1c12d0734f1172b 100644
--- a/src/processor/motion_buffer.cpp
+++ b/src/processor/motion_buffer.cpp
@@ -3,15 +3,15 @@ namespace wolf
{
Motion::Motion(const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _delta,
- const MatrixXs& _delta_cov,
- const VectorXs& _delta_integr,
- const MatrixXs& _delta_integr_cov,
- const MatrixXs& _jac_delta,
- const MatrixXs& _jac_delta_int,
- const MatrixXs& _jac_calib) :
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _delta,
+ const MatrixXd& _delta_cov,
+ const VectorXd& _delta_integr,
+ const MatrixXd& _delta_integr_cov,
+ const MatrixXd& _jac_delta,
+ const MatrixXd& _jac_delta_int,
+ const MatrixXd& _jac_calib) :
data_size_(_data.size()),
delta_size_(_delta.size()),
cov_size_(_delta_cov.cols()),
diff --git a/src/processor/processor_diff_drive.cpp b/src/processor/processor_diff_drive.cpp
index 51c8a69915a2fe82dc368e03d60d5d6d013590ef..261bf68b6ee87ced523f8cba723101f1fae7820b 100644
--- a/src/processor/processor_diff_drive.cpp
+++ b/src/processor/processor_diff_drive.cpp
@@ -23,7 +23,7 @@ ProcessorDiffDrive::ProcessorDiffDrive(const ProcessorParamsDiffDrivePtr _params
{
setType("DIFF DRIVE"); // overwrite odom2D setting
calib_size_ = 3; // overwrite odom2D setting
- unmeasured_perturbation_cov_ = Matrix1s(_params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std); // overwrite odom2D setting
+ unmeasured_perturbation_cov_ = Matrix1d(_params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std); // overwrite odom2D setting
}
ProcessorDiffDrive::~ProcessorDiffDrive()
@@ -45,13 +45,13 @@ void ProcessorDiffDrive::configure(SensorBasePtr _sensor)
//////////// MOTION INTEGRATION ///////////////
-void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
+void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
const double _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) const
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const
{
/*
* Differential drive model -----------------------------------------------
@@ -73,9 +73,9 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXs& _data,
/// 1. Tangent space: calibrate and go to se2 tangent -----------------------------------------------
- Vector3s tangent(dl, 0, dth); // second value 'ds' is wheel sideways motion, supposed zero. Will have a covariance for allowing skidding.
+ Vector3d tangent(dl, 0, dth); // second value 'ds' is wheel sideways motion, supposed zero. Will have a covariance for allowing skidding.
- Matrix3s J_tangent_calib;
+ Matrix3d J_tangent_calib;
J_tangent_calib(0,0) = k * _data(0) / 2.0; // d dl / d r_l
J_tangent_calib(0,1) = k * _data(1) / 2.0; // d dl / d r_r
J_tangent_calib(0,2) = 0.0; // d dl / d d
@@ -101,7 +101,7 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXs& _data,
/// 2. Current delta: go to SE2 manifold -----------------------------------------------
- Matrix3s J_delta_tangent;
+ Matrix3d J_delta_tangent;
SE2::exp(tangent, _delta, J_delta_tangent);
@@ -126,10 +126,10 @@ void ProcessorDiffDrive::computeCurrentDelta(const Eigen::VectorXs& _data,
CaptureMotionPtr ProcessorDiffDrive::emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _calib,
- const VectorXs& _calib_preint,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin)
{
auto cap_motion = CaptureBase::emplace<CaptureDiffDrive>(_frame_own,
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 88b94628653ac76d025e3845ddccb88f3059da5a..10e1ffb9d5f1b6e5e918a453dca26c4227f9ba1e 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -55,7 +55,7 @@ void ProcessorMotion::splitBuffer(const wolf::CaptureMotionPtr& _capture_source,
_capture_source->setOriginCapture(_capture_target);
// Get optimized calibration params from 'origin' keyframe
- VectorXs calib_preint_optim = _capture_source->getOriginCapture()->getCalibration();
+ VectorXd calib_preint_optim = _capture_source->getOriginCapture()->getCalibration();
// Write the calib params into the capture before re-integration
_capture_source->setCalibrationPreint(calib_preint_optim);
@@ -129,14 +129,14 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto capture_origin = capture_existing->getOriginCapture();
// Get calibration params for preintegration from origin, since it has chances to have optimized values
- VectorXs calib_origin = capture_origin->getCalibration();
+ VectorXd calib_origin = capture_origin->getCalibration();
// emplace a new motion capture to the new keyframe
TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback, // j
getSensor(),
ts_from_callback,
- Eigen::VectorXs::Zero(data_size_),
+ Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
calib_origin,
calib_origin,
@@ -214,14 +214,14 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto & capture_origin = origin_ptr_;
// Get calibration params for preintegration from origin, since it has chances to have optimized values
- VectorXs calib_origin = capture_origin->getCalibration();
+ VectorXd calib_origin = capture_origin->getCalibration();
// emplace a new motion capture to the new keyframe
TimeStamp ts_from_callback = keyframe_from_callback->getTimeStamp();
auto capture_for_keyframe_callback = emplaceCapture(keyframe_from_callback,
getSensor(),
ts_from_callback,
- Eigen::VectorXs::Zero(data_size_),
+ Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
calib_origin,
calib_origin,
@@ -310,7 +310,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
auto capture_new = emplaceCapture(frame_new,
getSensor(),
key_frame->getTimeStamp(),
- Eigen::VectorXs::Zero(data_size_),
+ Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
last_ptr_->getCalibration(),
last_ptr_->getCalibration(),
@@ -335,7 +335,7 @@ void ProcessorMotion::processCapture(CaptureBasePtr _incoming_ptr)
postProcess();
}
-bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x) const
+bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXd& _x) const
{
CaptureMotionPtr capture_motion;
if (origin_ptr_ && _ts >= origin_ptr_->getTimeStamp())
@@ -349,15 +349,15 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x) const
{
// Get origin state and calibration
CaptureBasePtr cap_orig = capture_motion->getOriginCapture();
- VectorXs state_0 = cap_orig->getFrame()->getState();
- VectorXs calib = cap_orig->getCalibration();
+ VectorXd state_0 = cap_orig->getFrame()->getState();
+ VectorXd calib = cap_orig->getCalibration();
// Get delta and correct it with new calibration params
- VectorXs calib_preint = capture_motion->getCalibrationPreint();
+ VectorXd calib_preint = capture_motion->getCalibrationPreint();
Motion motion = capture_motion->getBuffer().getMotion(_ts);
- VectorXs delta_step = motion.jacobian_calib_ * (calib - calib_preint);
- VectorXs delta = capture_motion->correctDelta( motion.delta_integr_, delta_step);
+ VectorXd delta_step = motion.jacobian_calib_ * (calib - calib_preint);
+ VectorXd delta = capture_motion->correctDelta( motion.delta_integr_, delta_step);
// Compose on top of origin state using the buffered time stamp, not the query time stamp
double dt = motion.ts_ - capture_motion->getBuffer().get().front().ts_;
@@ -373,7 +373,7 @@ bool ProcessorMotion::getState(const TimeStamp& _ts, Eigen::VectorXs& _x) const
return true;
}
-FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXs& _x_origin, const TimeStamp& _ts_origin)
+FrameBasePtr ProcessorMotion::setOrigin(const Eigen::VectorXd& _x_origin, const TimeStamp& _ts_origin)
{
FrameBasePtr key_frame_ptr = getProblem()->emplaceFrame(KEY, _x_origin, _ts_origin);
setOrigin(key_frame_ptr);
@@ -393,7 +393,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
origin_ptr_ = emplaceCapture(_origin_frame,
getSensor(),
_origin_frame->getTimeStamp(),
- Eigen::VectorXs::Zero(data_size_),
+ Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
getSensor()->getCalibration(),
getSensor()->getCalibration(),
@@ -408,7 +408,7 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
last_ptr_ = emplaceCapture(new_frame_ptr,
getSensor(),
_origin_frame->getTimeStamp(),
- Eigen::VectorXs::Zero(data_size_),
+ Eigen::VectorXd::Zero(data_size_),
getSensor()->getNoiseCov(),
getSensor()->getCalibration(),
getSensor()->getCalibration(),
@@ -475,7 +475,7 @@ void ProcessorMotion::integrateOneStep()
void ProcessorMotion::reintegrateBuffer(CaptureMotionPtr _capture_ptr)
{
- VectorXs calib = _capture_ptr->getCalibrationPreint();
+ VectorXd calib = _capture_ptr->getCalibrationPreint();
// some temporaries for integration
delta_integrated_ =deltaZero();
diff --git a/src/processor/processor_odom_2D.cpp b/src/processor/processor_odom_2D.cpp
index d34739cd6ad5705cbed753cecedb5e90cd19d111..2a290fa4b1119a545b916f78a4a10addfd25a621 100644
--- a/src/processor/processor_odom_2D.cpp
+++ b/src/processor/processor_odom_2D.cpp
@@ -6,16 +6,16 @@ ProcessorOdom2D::ProcessorOdom2D(ProcessorParamsOdom2DPtr _params) :
ProcessorMotion("ODOM 2D", 3, 3, 3, 2, 0, _params),
params_odom_2D_(_params)
{
- unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3s::Identity();
+ unmeasured_perturbation_cov_ = _params->unmeasured_perturbation_std * _params->unmeasured_perturbation_std * Matrix3d::Identity();
}
ProcessorOdom2D::~ProcessorOdom2D()
{
}
-void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib, const double _dt, Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov, Eigen::MatrixXs& _jacobian_calib) const
+void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXd& _data, const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib, const double _dt, Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov, Eigen::MatrixXd& _jacobian_calib) const
{
assert(_data.size() == data_size_ && "Wrong _data vector size");
assert(_data_cov.rows() == data_size_ && "Wrong _data_cov size");
@@ -29,7 +29,7 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Ei
_delta(2) = _data(1);
// Fill delta covariance
- Eigen::MatrixXs J_delta_data(delta_cov_size_, data_size_);
+ Eigen::MatrixXd J_delta_data(delta_cov_size_, data_size_);
J_delta_data(0, 0) = cos(_data(1) / 2);
J_delta_data(1, 0) = sin(_data(1) / 2);
J_delta_data(2, 0) = 0;
@@ -43,21 +43,21 @@ void ProcessorOdom2D::computeCurrentDelta(const Eigen::VectorXs& _data, const Ei
_delta_cov = J_delta_data * _data_cov * J_delta_data.transpose() + unmeasured_perturbation_cov_;
}
-void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const double _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2) const
+void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const
{
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
// This is just a frame composition in 2D
- _delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
+ _delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Dd(_delta1(2)).matrix() * _delta2.head<2>();
_delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
}
-void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const double _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2, Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2) const
+void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2, Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const
{
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
@@ -68,27 +68,27 @@ void ProcessorOdom2D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen
assert(_jacobian2.cols() == delta_cov_size_ && "Wrong _jacobian2 size");
// This is just a frame composition in 2D
- _delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Ds(_delta1(2)).matrix() * _delta2.head<2>();
+ _delta1_plus_delta2.head<2>() = _delta1.head<2>() + Eigen::Rotation2Dd(_delta1(2)).matrix() * _delta2.head<2>();
_delta1_plus_delta2(2) = pi2pi(_delta1(2) + _delta2(2));
// Jac wrt delta_integrated
- _jacobian1 = Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_);
+ _jacobian1 = Eigen::MatrixXd::Identity(delta_cov_size_, delta_cov_size_);
_jacobian1(0, 2) = -sin(_delta1(2)) * _delta2(0) - cos(_delta1(2)) * _delta2(1);
_jacobian1(1, 2) = cos(_delta1(2)) * _delta2(0) - sin(_delta1(2)) * _delta2(1);
// jac wrt delta
- _jacobian2 = Eigen::MatrixXs::Identity(delta_cov_size_, delta_cov_size_);
- _jacobian2.topLeftCorner<2, 2>() = Eigen::Rotation2Ds(_delta1(2)).matrix();
+ _jacobian2 = Eigen::MatrixXd::Identity(delta_cov_size_, delta_cov_size_);
+ _jacobian2.topLeftCorner<2, 2>() = Eigen::Rotation2Dd(_delta1(2)).matrix();
}
-void ProcessorOdom2D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const double _Dt,
- Eigen::VectorXs& _x_plus_delta) const
+void ProcessorOdom2D::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, const double _Dt,
+ Eigen::VectorXd& _x_plus_delta) const
{
assert(_x.size() == x_size_ && "Wrong _x vector size");
assert(_x_plus_delta.size() == x_size_ && "Wrong _x_plus_delta vector size");
// This is just a frame composition in 2D
- _x_plus_delta.head<2>() = _x.head<2>() + Eigen::Rotation2Ds(_x(2)).matrix() * _delta.head<2>();
+ _x_plus_delta.head<2>() = _x.head<2>() + Eigen::Rotation2Dd(_x(2)).matrix() * _delta.head<2>();
_x_plus_delta(2) = pi2pi(_x(2) + _delta(2));
}
@@ -123,10 +123,10 @@ bool ProcessorOdom2D::voteForKeyFrame() const
CaptureMotionPtr ProcessorOdom2D::emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _calib,
- const VectorXs& _calib_preint,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin_ptr)
{
auto cap_motion = CaptureBase::emplace<CaptureOdom2D>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin_ptr);
@@ -147,7 +147,7 @@ FactorBasePtr ProcessorOdom2D::emplaceFactor(FeatureBasePtr _feature, CaptureBas
FeatureBasePtr ProcessorOdom2D::emplaceFeature(CaptureMotionPtr _capture_motion)
{
- Eigen::MatrixXs covariance = _capture_motion->getBuffer().get().back().delta_integr_cov_;
+ Eigen::MatrixXd covariance = _capture_motion->getBuffer().get().back().delta_integr_cov_;
makePosDef(covariance);
FeatureBasePtr key_feature_ptr = FeatureBase::emplace<FeatureBase>(_capture_motion,
diff --git a/src/processor/processor_odom_3D.cpp b/src/processor/processor_odom_3D.cpp
index 1c737bf7f2207e80b36192288257763d1d721d9d..4a45ebf36ffadf3ea03c62e25b21970ba190ba81 100644
--- a/src/processor/processor_odom_3D.cpp
+++ b/src/processor/processor_odom_3D.cpp
@@ -32,13 +32,13 @@ void ProcessorOdom3D::configure(SensorBasePtr _sensor)
min_rot_var_ = sen_ptr->getMinRotVar();
}
-void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
+void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
const double _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) const
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const
{
assert((_data.size() == 6 || _data.size() == 7) && "Wrong data size. Must be 6 or 7 for 3D.");
double disp, rot; // displacement and rotation of this motion step
@@ -46,7 +46,7 @@ void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXs& _data,
{
// rotation in vector form
_delta.head<3>() = _data.head<3>();
- Eigen::Map<Eigen::Quaternions> q(_delta.data() + 3);
+ Eigen::Map<Eigen::Quaterniond> q(_delta.data() + 3);
q = v2q(_data.tail<3>());
disp = _data.head<3>().norm();
rot = _data.tail<3>().norm();
@@ -75,15 +75,15 @@ void ProcessorOdom3D::computeCurrentDelta(const Eigen::VectorXs& _data,
// We discard _data_cov and create a new one from the measured motion
double disp_var = min_disp_var_ + k_disp_to_disp_ * disp;
double rot_var = min_rot_var_ + k_disp_to_rot_ * disp + k_rot_to_rot_ * rot;
- Eigen::Matrix6s data_cov(Eigen::Matrix6s::Identity());
+ Eigen::Matrix6d data_cov(Eigen::Matrix6d::Identity());
data_cov(0, 0) = data_cov(1, 1) = data_cov(2, 2) = disp_var;
data_cov(3, 3) = data_cov(4, 4) = data_cov(5, 5) = rot_var;
_delta_cov = data_cov;
}
-void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const double _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2, Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2) const
+void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2, Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const
{
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
@@ -91,12 +91,12 @@ void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen
assert(_jacobian1.rows() == delta_cov_size_ && _jacobian1.cols() == delta_cov_size_ && "Wrong _jacobian1 size");
assert(_jacobian2.rows() == delta_cov_size_ && _jacobian2.cols() == delta_cov_size_ && "Wrong _jacobian2 size");
- Eigen::Map<const Eigen::Vector3s> dp1 (_delta1.data() );
- Eigen::Map<const Eigen::Quaternions> dq1 (_delta1.data() + 3 );
- Eigen::Map<const Eigen::Vector3s> dp2 (_delta2.data() );
- Eigen::Map<const Eigen::Quaternions> dq2 (_delta2.data() + 3 );
- Eigen::Map<Eigen::Vector3s> dp_out (_delta1_plus_delta2.data() );
- Eigen::Map<Eigen::Quaternions> dq_out (_delta1_plus_delta2.data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> dp1 (_delta1.data() );
+ Eigen::Map<const Eigen::Quaterniond> dq1 (_delta1.data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> dp2 (_delta2.data() );
+ Eigen::Map<const Eigen::Quaterniond> dq2 (_delta2.data() + 3 );
+ Eigen::Map<Eigen::Vector3d> dp_out (_delta1_plus_delta2.data() );
+ Eigen::Map<Eigen::Quaterniond> dq_out (_delta1_plus_delta2.data() + 3 );
/* Jacobians of D' = D (+) d
* with: D = [Dp Dq]
@@ -114,8 +114,8 @@ void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen
*/
// temporaries
- Eigen::Matrix3s dR1 = dq1.matrix();
- Eigen::Matrix3s dR2 = dq2.matrix();
+ Eigen::Matrix3d dR1 = dq1.matrix();
+ Eigen::Matrix3d dR2 = dq2.matrix();
// fill Jacobians
_jacobian1.setIdentity();
@@ -130,38 +130,38 @@ void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen
dq_out = dq1 * dq2;
}
-void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXs& _delta1, const Eigen::VectorXs& _delta2, const double _Dt2,
- Eigen::VectorXs& _delta1_plus_delta2) const
+void ProcessorOdom3D::deltaPlusDelta(const Eigen::VectorXd& _delta1, const Eigen::VectorXd& _delta2, const double _Dt2,
+ Eigen::VectorXd& _delta1_plus_delta2) const
{
assert(_delta1.size() == delta_size_ && "Wrong _delta1 vector size");
assert(_delta2.size() == delta_size_ && "Wrong _delta2 vector size");
assert(_delta1_plus_delta2.size() == delta_size_ && "Wrong _delta1_plus_delta2 vector size");
- Eigen::Map<const Eigen::Vector3s> dp1 (_delta1.data() );
- Eigen::Map<const Eigen::Quaternions> dq1 (_delta1.data() + 3 );
- Eigen::Map<const Eigen::Vector3s> dp2 (_delta2.data() );
- Eigen::Map<const Eigen::Quaternions> dq2 (_delta2.data() + 3 );
- Eigen::Map<Eigen::Vector3s> dp_out (_delta1_plus_delta2.data() );
- Eigen::Map<Eigen::Quaternions> dq_out (_delta1_plus_delta2.data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> dp1 (_delta1.data() );
+ Eigen::Map<const Eigen::Quaterniond> dq1 (_delta1.data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> dp2 (_delta2.data() );
+ Eigen::Map<const Eigen::Quaterniond> dq2 (_delta2.data() + 3 );
+ Eigen::Map<Eigen::Vector3d> dp_out (_delta1_plus_delta2.data() );
+ Eigen::Map<Eigen::Quaterniond> dq_out (_delta1_plus_delta2.data() + 3 );
dp_out = dp1 + dq1 * dp2;
dq_out = dq1 * dq2;
}
-void ProcessorOdom3D::statePlusDelta(const Eigen::VectorXs& _x, const Eigen::VectorXs& _delta, const double _Dt,
- Eigen::VectorXs& _x_plus_delta) const
+void ProcessorOdom3D::statePlusDelta(const Eigen::VectorXd& _x, const Eigen::VectorXd& _delta, const double _Dt,
+ Eigen::VectorXd& _x_plus_delta) const
{
assert(_x.size() >= x_size_ && "Wrong _x vector size"); //we need a state vector which size is at least x_size_
assert(_delta.size() == delta_size_ && "Wrong _delta vector size");
assert(_x_plus_delta.size() >= x_size_ && "Wrong _x_plus_delta vector size");
- Eigen::Map<const Eigen::Vector3s> p (_x.data() );
- Eigen::Map<const Eigen::Quaternions> q (_x.data() + 3 );
- Eigen::Map<const Eigen::Vector3s> dp (_delta.data() );
- Eigen::Map<const Eigen::Quaternions> dq (_delta.data() + 3 );
- Eigen::Map<Eigen::Vector3s> p_out (_x_plus_delta.data() );
- Eigen::Map<Eigen::Quaternions> q_out (_x_plus_delta.data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> p (_x.data() );
+ Eigen::Map<const Eigen::Quaterniond> q (_x.data() + 3 );
+ Eigen::Map<const Eigen::Vector3d> dp (_delta.data() );
+ Eigen::Map<const Eigen::Quaterniond> dq (_delta.data() + 3 );
+ Eigen::Map<Eigen::Vector3d> p_out (_x_plus_delta.data() );
+ Eigen::Map<Eigen::Quaterniond> q_out (_x_plus_delta.data() + 3 );
p_out = p + q * dp;
@@ -198,7 +198,7 @@ bool ProcessorOdom3D::voteForKeyFrame() const
return true;
}
// angle turned
- double angle = q2v(Quaternions(getMotion().delta_integr_.data()+3)).norm(); // 2.0 * acos(getMotion().delta_integr_(6));
+ double angle = q2v(Quaterniond(getMotion().delta_integr_.data()+3)).norm(); // 2.0 * acos(getMotion().delta_integr_(6));
if (angle > params_odom_3D_->angle_turned)
{
WOLF_DEBUG( "PM: vote: angle turned" );
@@ -211,10 +211,10 @@ bool ProcessorOdom3D::voteForKeyFrame() const
CaptureMotionPtr ProcessorOdom3D::emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _calib,
- const VectorXs& _calib_preint,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin)
{
auto cap_motion = CaptureBase::emplace<CaptureOdom3D>(_frame_own, _ts, _sensor, _data, _data_cov, _capture_origin);
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 755a5d7e02864db5b180c6e65bb824bedb65e759..e705b40f5c757b1729b425931827e64e99e6a96e 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -39,7 +39,7 @@ SensorBase::SensorBase(const std::string& _type,
StateBlockPtr _p_ptr,
StateBlockPtr _o_ptr,
StateBlockPtr _intr_ptr,
- const Eigen::VectorXs & _noise_std,
+ const Eigen::VectorXd & _noise_std,
const bool _extr_dyn,
const bool _intr_dyn) :
NodeBase("SENSOR", _type),
@@ -142,7 +142,7 @@ void SensorBase::unfixIntrinsics()
updateCalibSize();
}
-void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs& _x, const Eigen::MatrixXs& _cov, unsigned int _start_idx, int _size)
+void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXd& _x, const Eigen::MatrixXd& _cov, unsigned int _start_idx, int _size)
{
assert(!isStateBlockDynamic(_i) && "SensorBase::addPriorParameter only allowed for static parameters");
assert(_i < state_block_vec_.size() && "State block not found");
@@ -161,7 +161,7 @@ void SensorBase::addPriorParameter(const unsigned int _i, const Eigen::VectorXs&
_sb->setState(_x);
else
{
- Eigen::VectorXs new_x = _sb->getState();
+ Eigen::VectorXd new_x = _sb->getState();
new_x.segment(_start_idx,_size) = _x;
_sb->setState(new_x);
}
@@ -208,12 +208,12 @@ void SensorBase::registerNewStateBlocks()
}
}
-void SensorBase::setNoiseStd(const Eigen::VectorXs& _noise_std) {
+void SensorBase::setNoiseStd(const Eigen::VectorXd& _noise_std) {
noise_std_ = _noise_std;
noise_cov_ = _noise_std.array().square().matrix().asDiagonal();
}
-void SensorBase::setNoiseCov(const Eigen::MatrixXs& _noise_cov) {
+void SensorBase::setNoiseCov(const Eigen::MatrixXd& _noise_cov) {
noise_std_ = _noise_cov.diagonal().array().sqrt();
noise_cov_ = _noise_cov;
}
@@ -302,11 +302,11 @@ SizeEigen SensorBase::computeCalibSize() const
return sz;
}
-Eigen::VectorXs SensorBase::getCalibration() const
+Eigen::VectorXd SensorBase::getCalibration() const
{
SizeEigen index = 0;
SizeEigen sz = getCalibSize();
- Eigen::VectorXs calib(sz);
+ Eigen::VectorXd calib(sz);
for (unsigned int i = 0; i < state_block_vec_.size(); i++)
{
auto sb = getStateBlockPtrStatic(i);
diff --git a/src/sensor/sensor_diff_drive.cpp b/src/sensor/sensor_diff_drive.cpp
index 95a95babfad00d8839dcbaa53d773b9372f0931e..32b2467faf2582b56d0f886feac7a35af675ca41 100644
--- a/src/sensor/sensor_diff_drive.cpp
+++ b/src/sensor/sensor_diff_drive.cpp
@@ -11,7 +11,7 @@
namespace wolf
{
-SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXs& _extrinsics,
+SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXd& _extrinsics,
IntrinsicsDiffDrivePtr _intrinsics) :
SensorBase("DIFF DRIVE",
std::make_shared<StateBlock>(_extrinsics.head(2), true),
@@ -21,7 +21,7 @@ SensorDiffDrive::SensorDiffDrive(const Eigen::VectorXs& _extrinsics,
params_diff_drive_(_intrinsics)
{
radians_per_tick = 2.0*M_PI / params_diff_drive_->ticks_per_wheel_revolution;
- getIntrinsic()->setState(Eigen::Vector3s(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
+ getIntrinsic()->setState(Eigen::Vector3d(_intrinsics->radius_left,_intrinsics->radius_right,_intrinsics->wheel_separation));
getIntrinsic()->unfix();
}
diff --git a/src/sensor/sensor_odom_2D.cpp b/src/sensor/sensor_odom_2D.cpp
index 1427728fb152ad2f305704c25947ebb63a9624a6..2bc7013e4df0ab8fc5810540162c75c840e168c4 100644
--- a/src/sensor/sensor_odom_2D.cpp
+++ b/src/sensor/sensor_odom_2D.cpp
@@ -4,7 +4,7 @@
namespace wolf {
-SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D& _intrinsics) :
+SensorOdom2D::SensorOdom2D(Eigen::VectorXd _extrinsics, const IntrinsicsOdom2D& _intrinsics) :
SensorBase("ODOM 2D", std::make_shared<StateBlock>(_extrinsics.head(2), true), std::make_shared<StateAngle>(_extrinsics(2), true), nullptr, 2),
k_disp_to_disp_(_intrinsics.k_disp_to_disp),
k_rot_to_rot_(_intrinsics.k_rot_to_rot)
@@ -13,7 +13,7 @@ SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, const IntrinsicsOdom2D&
//
}
-SensorOdom2D::SensorOdom2D(Eigen::VectorXs _extrinsics, IntrinsicsOdom2DPtr _intrinsics) :
+SensorOdom2D::SensorOdom2D(Eigen::VectorXd _extrinsics, IntrinsicsOdom2DPtr _intrinsics) :
SensorOdom2D(_extrinsics, *_intrinsics)
{
//
diff --git a/src/sensor/sensor_odom_3D.cpp b/src/sensor/sensor_odom_3D.cpp
index 04de2bbd642491004a640971dba01c99032c6607..8afc92cb8b2904d6e11e6c73e97eb9e37609346a 100644
--- a/src/sensor/sensor_odom_3D.cpp
+++ b/src/sensor/sensor_odom_3D.cpp
@@ -12,7 +12,7 @@
namespace wolf {
-SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const IntrinsicsOdom3D& _intrinsics) :
+SensorOdom3D::SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, const IntrinsicsOdom3D& _intrinsics) :
SensorBase("ODOM 3D", std::make_shared<StateBlock>(_extrinsics_pq.head(3), true), std::make_shared<StateQuaternion>(_extrinsics_pq.tail(4), true), nullptr, 6),
k_disp_to_disp_(_intrinsics.k_disp_to_disp),
k_disp_to_rot_(_intrinsics.k_disp_to_rot),
@@ -22,11 +22,11 @@ SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, const Intrinsi
{
assert(_extrinsics_pq.size() == 7 && "Bad extrinsics vector size! Should be 7 for 3D.");
- noise_cov_ = (Eigen::Vector6s() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal();
+ noise_cov_ = (Eigen::Vector6d() << min_disp_var_, min_disp_var_, min_disp_var_, min_rot_var_, min_rot_var_, min_rot_var_).finished().asDiagonal();
setNoiseCov(noise_cov_); // sets also noise_std_
}
-SensorOdom3D::SensorOdom3D(const Eigen::VectorXs& _extrinsics_pq, IntrinsicsOdom3DPtr _intrinsics) :
+SensorOdom3D::SensorOdom3D(const Eigen::VectorXd& _extrinsics_pq, IntrinsicsOdom3DPtr _intrinsics) :
SensorOdom3D(_extrinsics_pq, *_intrinsics)
{
//
diff --git a/src/solver/solver_manager.cpp b/src/solver/solver_manager.cpp
index ec2568d8f190e31140866d0e8504e5c1e4517d31..0d47cd191f51c51126d38a591fea7de3614f53ae 100644
--- a/src/solver/solver_manager.cpp
+++ b/src/solver/solver_manager.cpp
@@ -89,7 +89,7 @@ void SolverManager::update()
// state update
if (state_ptr->stateUpdated())
{
- Eigen::VectorXs new_state = state_ptr->getState();
+ Eigen::VectorXd new_state = state_ptr->getState();
// We assume the same size for the states in both WOLF and the solver.
std::copy(new_state.data(),new_state.data()+new_state.size(),getAssociatedMemBlockPtr(state_ptr));
// reset flag
@@ -128,7 +128,7 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
/// @todo whatif someone has changed the state notification during opti ??
/// JV: I do not see a problem here, the solver provides the optimal state given the factors, if someone changed the state during optimization, it will be overwritten by the optimal one.
- std::map<StateBlockPtr, Eigen::VectorXs>::iterator it = state_blocks_.begin(),
+ std::map<StateBlockPtr, Eigen::VectorXd>::iterator it = state_blocks_.begin(),
it_end = state_blocks_.end();
for (; it != it_end; ++it)
{
@@ -140,7 +140,7 @@ std::string SolverManager::solve(const ReportVerbosity report_level)
return report;
}
-Eigen::VectorXs& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state_ptr)
+Eigen::VectorXd& SolverManager::getAssociatedMemBlock(const StateBlockPtr& state_ptr)
{
auto it = state_blocks_.find(state_ptr);
diff --git a/src/solver_suitesparse/solver_manager.cpp b/src/solver_suitesparse/solver_manager.cpp
index 05d89142a35cc740c69c29712565d132d69fbe93..805307fcd4ba78aee0b7d73ceba8a700c67a59cf 100644
--- a/src/solver_suitesparse/solver_manager.cpp
+++ b/src/solver_suitesparse/solver_manager.cpp
@@ -61,7 +61,7 @@ void SolverManager::update(const WolfProblemPtr _problem_ptr)
void SolverManager::addFactor(FactorBasePtr _corr_ptr)
{
- //TODO MatrixXs J; Vector e;
+ //TODO MatrixXd J; Vector e;
// getResidualsAndJacobian(_corr_ptr, J, e);
// solverQR->addFactor(_corr_ptr, J, e);
diff --git a/src/state_block/local_parametrization_homogeneous.cpp b/src/state_block/local_parametrization_homogeneous.cpp
index 9fdc26e1c0a799d7e669c54755f1ed55b834268e..8fbf3ce24f9394ca46d3bfbb90355f6d2e1a7d37 100644
--- a/src/state_block/local_parametrization_homogeneous.cpp
+++ b/src/state_block/local_parametrization_homogeneous.cpp
@@ -22,9 +22,9 @@ LocalParametrizationHomogeneous::~LocalParametrizationHomogeneous()
//
}
-bool LocalParametrizationHomogeneous::plus(Eigen::Map<const Eigen::VectorXs>& _h,
- Eigen::Map<const Eigen::VectorXs>& _delta,
- Eigen::Map<Eigen::VectorXs>& _h_plus_delta) const
+bool LocalParametrizationHomogeneous::plus(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<const Eigen::VectorXd>& _delta,
+ Eigen::Map<Eigen::VectorXd>& _h_plus_delta) const
{
assert(_h.size() == global_size_ && "Wrong size of input homogeneous point.");
assert(_delta.size() == local_size_ && "Wrong size of input delta.");
@@ -32,18 +32,18 @@ bool LocalParametrizationHomogeneous::plus(Eigen::Map<const Eigen::VectorXs>& _h
using namespace Eigen;
- _h_plus_delta = ( exp_q(_delta) * Quaternions(_h.data()) ).coeffs();
+ _h_plus_delta = ( exp_q(_delta) * Quaterniond(_h.data()) ).coeffs();
return true;
}
-bool LocalParametrizationHomogeneous::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _h,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
+bool LocalParametrizationHomogeneous::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _h,
+ Eigen::Map<Eigen::MatrixXd>& _jacobian) const
{
assert(_h.size() == global_size_ && "Wrong size of input quaternion.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
- Eigen::Vector4s hh = _h/2;
+ Eigen::Vector4d hh = _h/2;
_jacobian << hh(3), hh(2), -hh(1),
-hh(2), hh(3), hh(0),
hh(1), -hh(0), hh(3),
@@ -51,12 +51,12 @@ bool LocalParametrizationHomogeneous::computeJacobian(Eigen::Map<const Eigen::Ve
return true;
}
-bool LocalParametrizationHomogeneous::minus(Eigen::Map<const Eigen::VectorXs>& _h1,
- Eigen::Map<const Eigen::VectorXs>& _h2,
- Eigen::Map<Eigen::VectorXs>& _h2_minus_h1)
+bool LocalParametrizationHomogeneous::minus(Eigen::Map<const Eigen::VectorXd>& _h1,
+ Eigen::Map<const Eigen::VectorXd>& _h2,
+ Eigen::Map<Eigen::VectorXd>& _h2_minus_h1)
{
- using Eigen::Quaternions;
- _h2_minus_h1 = log_q(Quaternions(_h2.data()) * Quaternions(_h1.data()).conjugate());
+ using Eigen::Quaterniond;
+ _h2_minus_h1 = log_q(Quaterniond(_h2.data()) * Quaterniond(_h1.data()).conjugate());
return true;
}
diff --git a/src/state_block/local_parametrization_quaternion.cpp b/src/state_block/local_parametrization_quaternion.cpp
index d8bcd6a0d1f9ffa02d77d754d389a4a711596137..bc9d702739fb0b99b9558b959dad9ff8aae88aee 100644
--- a/src/state_block/local_parametrization_quaternion.cpp
+++ b/src/state_block/local_parametrization_quaternion.cpp
@@ -8,9 +8,9 @@ namespace wolf {
////////// LOCAL PERTURBATION //////////////
template <>
-bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::VectorXs>& _q,
- Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const
+bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::VectorXd>& _q,
+ Eigen::Map<const Eigen::VectorXd>& _delta_theta,
+ Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
@@ -21,19 +21,19 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::plus(Eigen::Map<const Eigen::Vect
using namespace Eigen;
- _q_plus_delta_theta = ( Quaternions(_q.data()) * exp_q(_delta_theta) ).coeffs();
+ _q_plus_delta_theta = ( Quaterniond(_q.data()) * exp_q(_delta_theta) ).coeffs();
return true;
}
template <>
-bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _q,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
+bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
+ Eigen::Map<Eigen::MatrixXd>& _jacobian) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
- Eigen::Vector4s qq = _q/2;
+ Eigen::Vector4d qq = _q/2;
_jacobian << qq(3), -qq(2), qq(1),
qq(2), qq(3), -qq(0),
-qq(1), qq(0), qq(3),
@@ -43,16 +43,16 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::computeJacobian(Eigen::Map<const
}
template <>
-bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::VectorXs>& _q1,
- Eigen::Map<const Eigen::VectorXs>& _q2,
- Eigen::Map<Eigen::VectorXs>& _q2_minus_q1)
+bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::VectorXd>& _q1,
+ Eigen::Map<const Eigen::VectorXd>& _q2,
+ Eigen::Map<Eigen::VectorXd>& _q2_minus_q1)
{
assert(_q1.size() == global_size_ && "Wrong size of input quaternion.");
assert(_q2.size() == global_size_ && "Wrong size of input quaternion.");
assert(_q2_minus_q1.size() == local_size_ && "Wrong size of output quaternion difference.");
- using Eigen::Quaternions;
- _q2_minus_q1 = log_q(Quaternions(_q1.data()).conjugate() * Quaternions(_q2.data()));
+ using Eigen::Quaterniond;
+ _q2_minus_q1 = log_q(Quaterniond(_q1.data()).conjugate() * Quaterniond(_q2.data()));
return true;
}
@@ -60,9 +60,9 @@ bool LocalParametrizationQuaternion<DQ_LOCAL>::minus(Eigen::Map<const Eigen::Vec
////////// GLOBAL PERTURBATION //////////////
template <>
-bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::VectorXs>& _q,
- Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _q_plus_delta_theta) const
+bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::VectorXd>& _q,
+ Eigen::Map<const Eigen::VectorXd>& _delta_theta,
+ Eigen::Map<Eigen::VectorXd>& _q_plus_delta_theta) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
@@ -73,19 +73,19 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::plus(Eigen::Map<const Eigen::Vec
using namespace Eigen;
- _q_plus_delta_theta = ( exp_q(_delta_theta) * Quaternions(_q.data()) ).coeffs();
+ _q_plus_delta_theta = ( exp_q(_delta_theta) * Quaterniond(_q.data()) ).coeffs();
return true;
}
template <>
-bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _q,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
+bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _q,
+ Eigen::Map<Eigen::MatrixXd>& _jacobian) const
{
assert(_q.size() == global_size_ && "Wrong size of input quaternion.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
- Eigen::Vector4s qq = _q/2;
+ Eigen::Vector4d qq = _q/2;
_jacobian << qq(3), qq(2), -qq(1),
-qq(2), qq(3), qq(0),
qq(1), -qq(0), qq(3),
@@ -95,16 +95,16 @@ bool LocalParametrizationQuaternion<DQ_GLOBAL>::computeJacobian(Eigen::Map<const
}
template <>
-bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::VectorXs>& _q1,
- Eigen::Map<const Eigen::VectorXs>& _q2,
- Eigen::Map<Eigen::VectorXs>& _q2_minus_q1)
+bool LocalParametrizationQuaternion<DQ_GLOBAL>::minus(Eigen::Map<const Eigen::VectorXd>& _q1,
+ Eigen::Map<const Eigen::VectorXd>& _q2,
+ Eigen::Map<Eigen::VectorXd>& _q2_minus_q1)
{
assert(_q1.size() == global_size_ && "Wrong size of input quaternion.");
assert(_q2.size() == global_size_ && "Wrong size of input quaternion.");
assert(_q2_minus_q1.size() == local_size_ && "Wrong size of output quaternion difference.");
- using Eigen::Quaternions;
- _q2_minus_q1 = log_q(Quaternions(_q2.data()) * Quaternions(_q1.data()).conjugate());
+ using Eigen::Quaterniond;
+ _q2_minus_q1 = log_q(Quaterniond(_q2.data()) * Quaterniond(_q1.data()).conjugate());
return true;
}
diff --git a/src/state_block/state_block.cpp b/src/state_block/state_block.cpp
index e37f70ff44a22de9873ab1dad510a4ba1b1232d9..f582ae97787b4a3df8d1e98177385427580a90c0 100644
--- a/src/state_block/state_block.cpp
+++ b/src/state_block/state_block.cpp
@@ -2,7 +2,7 @@
namespace wolf
{
-void StateBlock::setState(const Eigen::VectorXs& _state, const bool _notify)
+void StateBlock::setState(const Eigen::VectorXd& _state, const bool _notify)
{
assert(_state.size() == state_.size());
{
diff --git a/test/dummy/factor_feature_dummy.h b/test/dummy/factor_feature_dummy.h
index 3079537131bade42dcc68994fdb4ce45383a43db..55c2c5fa3c5792340e31d03e71a4764e753df105 100644
--- a/test/dummy/factor_feature_dummy.h
+++ b/test/dummy/factor_feature_dummy.h
@@ -24,7 +24,7 @@ class FactorFeatureDummy : public FactorBase
/** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
**/
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const override {};
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override {};
/** \brief Returns the jacobians computation method
**/
diff --git a/test/dummy/factor_landmark_dummy.h b/test/dummy/factor_landmark_dummy.h
index ab316fdb02c00dba737552fa70cc2eece1580ab3..cb8ec7497cf78f810cd76782b33da194ff6a54b3 100644
--- a/test/dummy/factor_landmark_dummy.h
+++ b/test/dummy/factor_landmark_dummy.h
@@ -24,7 +24,7 @@ class FactorLandmarkDummy : public FactorBase
/** Returns a residual vector and a vector of Jacobian matrix corresponding to each state block evaluated in the point provided in _states_ptr
**/
- virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXs& residual_, std::vector<Eigen::MatrixXs>& jacobians_) const override {};
+ virtual void evaluate(const std::vector<const double*>& _states_ptr, Eigen::VectorXd& residual_, std::vector<Eigen::MatrixXd>& jacobians_) const override {};
/** \brief Returns the jacobians computation method
**/
diff --git a/test/dummy/processor_tracker_feature_dummy.cpp b/test/dummy/processor_tracker_feature_dummy.cpp
index 05b2f34494472cfa9fde355b91d0b48a6609d25a..377edd5badd79c67a0471d4392576303726bb81a 100644
--- a/test/dummy/processor_tracker_feature_dummy.cpp
+++ b/test/dummy/processor_tracker_feature_dummy.cpp
@@ -71,8 +71,8 @@ unsigned int ProcessorTrackerFeatureDummy::detectNewFeatures(const int& _max_new
{
FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(_capture,
"DUMMY FEATURE",
- Eigen::Vector1s::Ones(),
- Eigen::MatrixXs::Ones(1, 1));
+ Eigen::Vector1d::Ones(),
+ Eigen::MatrixXd::Ones(1, 1));
_features_out.push_back(ftr);
WOLF_INFO("feature " , ftr->id() , " detected!" );
diff --git a/test/dummy/processor_tracker_landmark_dummy.cpp b/test/dummy/processor_tracker_landmark_dummy.cpp
index 532906ac27f319486b619fd6f8e0f882d3676197..2c5cba07419419bc39ca6ec49cbcb474f590c680 100644
--- a/test/dummy/processor_tracker_landmark_dummy.cpp
+++ b/test/dummy/processor_tracker_landmark_dummy.cpp
@@ -43,8 +43,8 @@ unsigned int ProcessorTrackerLandmarkDummy::findLandmarks(const LandmarkBasePtrL
{
FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(_capture,
"DUMMY FEATURE",
- Eigen::Vector1s::Ones(),
- Eigen::MatrixXs::Ones(1, 1));
+ Eigen::Vector1d::Ones(),
+ Eigen::MatrixXd::Ones(1, 1));
_features_out.push_back(ftr);
_feature_landmark_correspondences[ftr] = std::make_shared<LandmarkMatch>(landmark_in_ptr, 1);
std::cout << "\t\tlandmark " << landmark_in_ptr->id() << " found!" << std::endl;
@@ -86,8 +86,8 @@ unsigned int ProcessorTrackerLandmarkDummy::detectNewFeatures(const int& _max_fe
{
FeatureBasePtr ftr = FeatureBase::emplace<FeatureBase>(_capture,
"DUMMY FEATURE",
- Eigen::Vector1s::Ones(),
- Eigen::MatrixXs::Ones(1, 1));
+ Eigen::Vector1d::Ones(),
+ Eigen::MatrixXd::Ones(1, 1));
_features_out.push_back(ftr);
WOLF_INFO("feature " , ftr->id() , " detected!" );
diff --git a/test/gtest_SE3.cpp b/test/gtest_SE3.cpp
index b25aea7faec5ab83747459a7c89e60f013f28e84..329feed0d0aad5565204f4b746cc1781ec872c9d 100644
--- a/test/gtest_SE3.cpp
+++ b/test/gtest_SE3.cpp
@@ -17,24 +17,24 @@ using namespace SE3;
TEST(SE3, exp_0)
{
- Vector6s tau = Vector6s::Zero();
- Vector7s pose; pose << 0,0,0, 0,0,0,1;
+ Vector6d tau = Vector6d::Zero();
+ Vector7d pose; pose << 0,0,0, 0,0,0,1;
ASSERT_MATRIX_APPROX(pose, exp_SE3(tau), 1e-8);
}
TEST(SE3, log_I)
{
- Vector7s pose; pose << 0,0,0, 0,0,0,1;
- Vector6s tau = Vector6s::Zero();
+ Vector7d pose; pose << 0,0,0, 0,0,0,1;
+ Vector6d tau = Vector6d::Zero();
ASSERT_MATRIX_APPROX(tau, log_SE3(pose), 1e-8);
}
TEST(SE3, exp_log)
{
- Vector6s tau = Vector6s::Random() / 5.0;
- Vector7s pose = exp_SE3(tau);
+ Vector6d tau = Vector6d::Random() / 5.0;
+ Vector7d pose = exp_SE3(tau);
ASSERT_MATRIX_APPROX(tau, log_SE3(exp_SE3(tau)), 1e-8);
ASSERT_MATRIX_APPROX(pose, exp_SE3(log_SE3(pose)), 1e-8);
@@ -42,9 +42,9 @@ TEST(SE3, exp_log)
TEST(SE3, exp_of_multiple)
{
- Vector6s tau, tau2, tau3;
- Vector7s pose, pose2, pose3;
- tau = Vector6s::Random() / 5;
+ Vector6d tau, tau2, tau3;
+ Vector7d pose, pose2, pose3;
+ tau = Vector6d::Random() / 5;
pose << exp_SE3(tau);
tau2 = 2*tau;
tau3 = 3*tau;
@@ -63,9 +63,9 @@ TEST(SE3, exp_of_multiple)
TEST(SE3, log_of_power)
{
- Vector6s tau, tau2, tau3;
- Vector7s pose, pose2, pose3;
- tau = Vector6s::Random() / 5;
+ Vector6d tau, tau2, tau3;
+ Vector7d pose, pose2, pose3;
+ tau = Vector6d::Random() / 5;
pose << exp_SE3(tau);
tau2 = 2*tau;
tau3 = 3*tau;
@@ -84,7 +84,7 @@ TEST(SE3, log_of_power)
TEST(SE3, interpolate_I_xyz)
{
- Vector7s p1, p2, p_pre;
+ Vector7d p1, p2, p_pre;
p1 << 0,0,0, 0,0,0,1;
p2 << 1,2,3, 0,0,0,1;
@@ -92,14 +92,14 @@ TEST(SE3, interpolate_I_xyz)
interpolate(p1, p2, t, p_pre);
- Vector7s p_gt; p_gt << 0.2,0.4,0.6, 0,0,0,1;
+ Vector7d p_gt; p_gt << 0.2,0.4,0.6, 0,0,0,1;
ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
}
TEST(SE3, interpolate_xyz_xyz)
{
- Vector7s p1, p2, p_pre;
+ Vector7d p1, p2, p_pre;
p1 << 1,2,3, 0,0,0,1;
p2 << 2,4,6, 0,0,0,1;
@@ -107,14 +107,14 @@ TEST(SE3, interpolate_xyz_xyz)
interpolate(p1, p2, t, p_pre);
- Vector7s p_gt; p_gt << 1.2,2.4,3.6, 0,0,0,1;
+ Vector7d p_gt; p_gt << 1.2,2.4,3.6, 0,0,0,1;
ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
}
TEST(SE3, interpolate_I_qx)
{
- Vector7s p1, p2, p_pre;
+ Vector7d p1, p2, p_pre;
p1 << 0,0,0, 0,0,0,1;
p2 << 0,0,0, 1,0,0,0;
@@ -122,14 +122,14 @@ TEST(SE3, interpolate_I_qx)
interpolate(p1, p2, t, p_pre);
- Vector7s p_gt; p_gt << 0,0,0, sqrt(2)/2,0,0,sqrt(2)/2;
+ Vector7d p_gt; p_gt << 0,0,0, sqrt(2)/2,0,0,sqrt(2)/2;
ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
}
TEST(SE3, interpolate_I_qy)
{
- Vector7s p1, p2, p_pre;
+ Vector7d p1, p2, p_pre;
p1 << 0,0,0, 0,0,0,1;
p2 << 0,0,0, 0,1,0,0;
@@ -137,14 +137,14 @@ TEST(SE3, interpolate_I_qy)
interpolate(p1, p2, t, p_pre);
- Vector7s p_gt; p_gt << 0,0,0, 0,sqrt(2)/2,0,sqrt(2)/2;
+ Vector7d p_gt; p_gt << 0,0,0, 0,sqrt(2)/2,0,sqrt(2)/2;
ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
}
TEST(SE3, interpolate_I_qz)
{
- Vector7s p1, p2, p_pre;
+ Vector7d p1, p2, p_pre;
p1 << 0,0,0, 0,0,0,1;
p2 << 0,0,0, 0,0,1,0;
@@ -152,15 +152,15 @@ TEST(SE3, interpolate_I_qz)
interpolate(p1, p2, t, p_pre);
- Vector7s p_gt; p_gt << 0,0,0, 0,0,sqrt(2)/2,sqrt(2)/2;
+ Vector7d p_gt; p_gt << 0,0,0, 0,0,sqrt(2)/2,sqrt(2)/2;
ASSERT_MATRIX_APPROX(p_pre, p_gt, 1e-8);
}
TEST(SE3, interpolate_I_qxyz)
{
- Vector7s p1, p2, dp, p_pre, p_gt;
- Vector3s da;
+ Vector7d p1, p2, dp, p_pre, p_gt;
+ Vector3d da;
da.setRandom(); da /= 5;
@@ -177,8 +177,8 @@ TEST(SE3, interpolate_I_qxyz)
TEST(SE3, interpolate_half)
{
- Vector7s p1, p2, dp, p_pre, p_gt;
- Vector6s da;
+ Vector7d p1, p2, dp, p_pre, p_gt;
+ Vector6d da;
p1.setRandom(); p1.tail(4).normalize();
@@ -198,8 +198,8 @@ TEST(SE3, interpolate_half)
TEST(SE3, interpolate_third)
{
- Vector7s p1, p2, dp, dp2, dp3, p_pre, p_gt;
- Vector6s da;
+ Vector7d p1, p2, dp, dp2, dp3, p_pre, p_gt;
+ Vector6d da;
p1.setRandom(); p1.tail(4).normalize();
@@ -221,11 +221,11 @@ TEST(SE3, interpolate_third)
TEST(SE3, toSE3_I)
{
- Vector7s pose; pose << 0,0,0, 0,0,0,1;
- Matrix4s R;
+ Vector7d pose; pose << 0,0,0, 0,0,0,1;
+ Matrix4d R;
toSE3(pose, R);
- ASSERT_MATRIX_APPROX(R, Matrix4s::Identity(), 1e-8);
+ ASSERT_MATRIX_APPROX(R, Matrix4d::Identity(), 1e-8);
}
int main(int argc, char **argv)
diff --git a/test/gtest_capture_base.cpp b/test/gtest_capture_base.cpp
index 15be1ff455614ce95b116e865b1666a1a728d24b..0aaeff5402504100759a82bdf85878b6ceea4631 100644
--- a/test/gtest_capture_base.cpp
+++ b/test/gtest_capture_base.cpp
@@ -76,8 +76,8 @@ TEST(CaptureBase, Dynamic_sensor_params)
TEST(CaptureBase, addFeature)
{
CaptureBasePtr C(std::make_shared<CaptureBase>("DUMMY", 1.2)); // timestamp = 1.2
- // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2s::Zero(), Matrix2s::Identity()));
- auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2s::Zero(), Matrix2s::Identity());
+ // FeatureBasePtr f = C->addFeature(std::make_shared<FeatureBase>("DUMMY", Vector2d::Zero(), Matrix2d::Identity()));
+ auto f = FeatureBase::emplace<FeatureBase>(C, "DUMMY", Vector2d::Zero(), Matrix2d::Identity());
ASSERT_FALSE(C->getFeatureList().empty());
ASSERT_EQ(C->getFeatureList().front(), f);
}
diff --git a/test/gtest_ceres_manager.cpp b/test/gtest_ceres_manager.cpp
index da5a01b80e98cf776957422d5bca8729b89ff038..f490fe349db4d0f8b0bd526be260cbae329f2c3f 100644
--- a/test/gtest_ceres_manager.cpp
+++ b/test/gtest_ceres_manager.cpp
@@ -97,7 +97,7 @@ TEST(CeresManager, AddStateBlock)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -120,7 +120,7 @@ TEST(CeresManager, DoubleAddStateBlock)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -149,7 +149,7 @@ TEST(CeresManager, UpdateStateBlock)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -180,7 +180,7 @@ TEST(CeresManager, AddUpdateStateBlock)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -207,7 +207,7 @@ TEST(CeresManager, RemoveStateBlock)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -239,7 +239,7 @@ TEST(CeresManager, AddRemoveStateBlock)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -265,7 +265,7 @@ TEST(CeresManager, RemoveUpdateStateBlock)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -290,7 +290,7 @@ TEST(CeresManager, DoubleRemoveStateBlock)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -320,7 +320,7 @@ TEST(CeresManager, AddFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// update solver
@@ -342,7 +342,7 @@ TEST(CeresManager, DoubleAddFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// add factor again
@@ -367,7 +367,7 @@ TEST(CeresManager, RemoveFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// update solver
@@ -395,7 +395,7 @@ TEST(CeresManager, AddRemoveFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// remove factor
@@ -422,7 +422,7 @@ TEST(CeresManager, DoubleRemoveFactor)
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f,f);
// update solver
@@ -455,7 +455,7 @@ TEST(CeresManager, AddStateBlockLocalParam)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
- Vector1s state; state << 1;
+ Vector1d state; state << 1;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Local param
@@ -482,7 +482,7 @@ TEST(CeresManager, RemoveLocalParam)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
- Vector1s state; state << 1;
+ Vector1d state; state << 1;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Local param
@@ -514,7 +514,7 @@ TEST(CeresManager, AddLocalParam)
CeresManagerWrapperPtr ceres_manager_ptr = std::make_shared<CeresManagerWrapper>(P);
// Create State block
- Vector1s state; state << 1;
+ Vector1d state; state << 1;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -549,7 +549,7 @@ TEST(CeresManager, FactorsRemoveLocalParam)
// Create (and add) 2 factors quaternion
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3d::Zero(), Matrix3d::Identity());
FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
@@ -591,7 +591,7 @@ TEST(CeresManager, FactorsUpdateLocalParam)
// Create (and add) 2 factors quaternion
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3d::Zero(), Matrix3d::Identity());
FactorQuaternionAbsolutePtr c1 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
FactorQuaternionAbsolutePtr c2 = FactorBase::emplace<FactorQuaternionAbsolute>(f, F->getO());
diff --git a/test/gtest_eigen_predicates.cpp b/test/gtest_eigen_predicates.cpp
index 48c53591eb60b4547bd8f8ba55f7aeff419780a2..036bc2a4b80ef4eeb1a38b146096ff1f1cd81487 100644
--- a/test/gtest_eigen_predicates.cpp
+++ b/test/gtest_eigen_predicates.cpp
@@ -4,11 +4,11 @@
TEST(TestEigenPredicates, TestEigenDynPredZero)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::MatrixXs::Zero(4,4);
- B = Eigen::MatrixXs::Random(4,4);
- C = Eigen::MatrixXs::Ones(4,4) * (wolf::Constants::EPS/2.);
+ A = Eigen::MatrixXd::Zero(4,4);
+ B = Eigen::MatrixXd::Random(4,4);
+ C = Eigen::MatrixXd::Ones(4,4) * (wolf::Constants::EPS/2.);
EXPECT_TRUE(wolf::pred_zero(A, wolf::Constants::EPS));
EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS));
@@ -19,11 +19,11 @@ TEST(TestEigenPredicates, TestEigenDynPredZero)
TEST(TestEigenPredicates, TestEigenFixPredZero)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::Matrix4s::Zero();
- B = Eigen::Matrix4s::Random();
- C = Eigen::Matrix4s::Ones() * (wolf::Constants::EPS/2.);
+ A = Eigen::Matrix4d::Zero();
+ B = Eigen::Matrix4d::Random();
+ C = Eigen::Matrix4d::Ones() * (wolf::Constants::EPS/2.);
EXPECT_TRUE(wolf::pred_zero(A, wolf::Constants::EPS));
EXPECT_FALSE(wolf::pred_zero(B, wolf::Constants::EPS));
@@ -34,10 +34,10 @@ TEST(TestEigenPredicates, TestEigenFixPredZero)
TEST(TestEigenPredicates, TestEigenDynPredDiffZero)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::MatrixXs::Random(4,4);
- B = Eigen::MatrixXs::Random(4,4);
+ A = Eigen::MatrixXd::Random(4,4);
+ B = Eigen::MatrixXd::Random(4,4);
C = A;
EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS));
@@ -48,10 +48,10 @@ TEST(TestEigenPredicates, TestEigenDynPredDiffZero)
TEST(TestEigenPredicates, TestEigenFixPredDiffZero)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::Matrix4s::Random();
- B = Eigen::Matrix4s::Random();
+ A = Eigen::Matrix4d::Random();
+ B = Eigen::Matrix4d::Random();
C = A;
EXPECT_TRUE(wolf::pred_diff_zero(A, C, wolf::Constants::EPS));
@@ -62,10 +62,10 @@ TEST(TestEigenPredicates, TestEigenFixPredDiffZero)
TEST(TestEigenPredicates, TestEigenDynPredDiffNorm)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::MatrixXs::Random(4,4);
- B = Eigen::MatrixXs::Random(4,4);
+ A = Eigen::MatrixXd::Random(4,4);
+ B = Eigen::MatrixXd::Random(4,4);
C = A;
EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS));
@@ -76,10 +76,10 @@ TEST(TestEigenPredicates, TestEigenDynPredDiffNorm)
TEST(TestEigenPredicates, TestEigenFixPredDiffNorm)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::Matrix4s::Random();
- B = Eigen::Matrix4s::Random();
+ A = Eigen::Matrix4d::Random();
+ B = Eigen::Matrix4d::Random();
C = A;
EXPECT_TRUE(wolf::pred_diff_norm_zero(A, C, wolf::Constants::EPS));
@@ -90,10 +90,10 @@ TEST(TestEigenPredicates, TestEigenFixPredDiffNorm)
TEST(TestEigenPredicates, TestEigenDynPredIsApprox)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::MatrixXs::Random(4,4);
- B = Eigen::MatrixXs::Random(4,4);
+ A = Eigen::MatrixXd::Random(4,4);
+ B = Eigen::MatrixXd::Random(4,4);
C = A;
EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS));
@@ -104,10 +104,10 @@ TEST(TestEigenPredicates, TestEigenDynPredIsApprox)
TEST(TestEigenPredicates, TestEigenFixPredIsApprox)
{
- Eigen::MatrixXs A, B, C;
+ Eigen::MatrixXd A, B, C;
- A = Eigen::Matrix4s::Random();
- B = Eigen::Matrix4s::Random();
+ A = Eigen::Matrix4d::Random();
+ B = Eigen::Matrix4d::Random();
C = A;
EXPECT_TRUE(wolf::pred_is_approx(A, C, wolf::Constants::EPS));
@@ -118,13 +118,13 @@ TEST(TestEigenPredicates, TestEigenFixPredIsApprox)
TEST(TestEigenPredicates, TestEigenPredQuatIsApprox)
{
- Eigen::Quaternions A, B, C;
+ Eigen::Quaterniond A, B, C;
/// @todo which version of Eigen provides this ?
-// A = Eigen::Quaternions::UnitRandom();
+// A = Eigen::Quaterniond::UnitRandom();
- A.coeffs() = Eigen::Vector4s::Random().normalized();
- B.coeffs() = Eigen::Vector4s::Random().normalized();
+ A.coeffs() = Eigen::Vector4d::Random().normalized();
+ B.coeffs() = Eigen::Vector4d::Random().normalized();
C = A;
EXPECT_TRUE(wolf::pred_quat_is_approx(A, C, wolf::Constants::EPS));
@@ -135,10 +135,10 @@ TEST(TestEigenPredicates, TestEigenPredQuatIsApprox)
TEST(TestEigenPredicates, TestEigenPredQuatIsIdentity)
{
- Eigen::Quaternions A, B;
+ Eigen::Quaterniond A, B;
- A = Eigen::Quaternions::Identity();
- B.coeffs() = Eigen::Vector4s::Random().normalized();
+ A = Eigen::Quaterniond::Identity();
+ B.coeffs() = Eigen::Vector4d::Random().normalized();
EXPECT_TRUE(wolf::pred_quat_identity(A, wolf::Constants::EPS));
EXPECT_FALSE(wolf::pred_quat_identity(B, wolf::Constants::EPS));
diff --git a/test/gtest_emplace.cpp b/test/gtest_emplace.cpp
index 13c209b8cd1631833c718f944c8d65803e93eaf7..7febd06735de0ee2cd6488a27621aa93e7fb7dde 100644
--- a/test/gtest_emplace.cpp
+++ b/test/gtest_emplace.cpp
@@ -88,8 +88,8 @@ TEST(Emplace, Feature)
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
- auto cov = Eigen::MatrixXs::Identity(2,2);
- FeatureBase::emplace<FeatureBase>(cpt, "Dummy", Eigen::VectorXs(2), cov);
+ auto cov = Eigen::MatrixXd::Identity(2,2);
+ FeatureBase::emplace<FeatureBase>(cpt, "Dummy", Eigen::VectorXd(2), cov);
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
@@ -106,8 +106,8 @@ TEST(Emplace, Factor)
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getProblem());
ASSERT_EQ(frm, frm->getCaptureList().front()->getFrame());
- auto cov = Eigen::MatrixXs::Identity(2,2);
- auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(2), cov);
+ auto cov = Eigen::MatrixXd::Identity(2,2);
+ auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXd(2), cov);
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getCapture()->getFrame()->getTrajectory()->getProblem());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
ASSERT_EQ(cpt, cpt->getFeatureList().front()->getCapture());
@@ -121,16 +121,16 @@ TEST(Emplace, EmplaceDerived)
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
// LandmarkBase::emplace<LandmarkBase>(MapBaseWPtr(P->getMap()),"Dummy", nullptr, nullptr);
- auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXs(3), IntrinsicsOdom2D());
- auto cov = Eigen::MatrixXs::Identity(2,2);
- auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::VectorXs(2), cov, nullptr);
+ auto sen = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Eigen::VectorXd(3), IntrinsicsOdom2D());
+ auto cov = Eigen::MatrixXd::Identity(2,2);
+ auto cpt = CaptureBase::emplace<CaptureOdom2D>(frm, TimeStamp(0), sen, Eigen::VectorXd(2), cov, nullptr);
// auto cpt2 = std::static_pointer_cast<CaptureOdom2D>(cpt);
auto m = Eigen::Matrix<double,9,6>();
for(int i = 0; i < 9; i++)
for(int j = 0; j < 6; j++)
m(i,j) = 1;
- auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(2), cov);
+ auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXd(2), cov);
ASSERT_EQ(sen, P->getHardware()->getSensorList().front());
ASSERT_EQ(P, P->getTrajectory()->getFrameList().front()->getCaptureList().front()->getFeatureList().front()->getProblem());
}
@@ -145,8 +145,8 @@ TEST(Emplace, ReturnDerived)
ASSERT_NE(P->getTrajectory(), nullptr);
auto frm = FrameBase::emplace<FrameBase>(P->getTrajectory(), KEY, TimeStamp(0), std::make_shared<StateBlock>(2,true), std::make_shared<StateBlock>(2,true));
auto cpt = CaptureBase::emplace<CaptureBase>(frm, "Dummy", TimeStamp(0), nullptr, nullptr, nullptr, nullptr);
- auto cov = Eigen::MatrixXs::Identity(2,2);
- auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXs(2), cov);
+ auto cov = Eigen::MatrixXd::Identity(2,2);
+ auto ftr = FeatureBase::emplace<FeatureOdom2D>(cpt, Eigen::VectorXd(2), cov);
auto cnt = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm);
FactorOdom2DPtr fac = FactorBase::emplace<FactorOdom2D>(ftr, ftr, frm);
diff --git a/test/gtest_factor_absolute.cpp b/test/gtest_factor_absolute.cpp
index 5e8d7d14bd065e7be00a18a37ca744d68b35a87a..5a229e140df444308d7a26f192a456020e01c13a 100644
--- a/test/gtest_factor_absolute.cpp
+++ b/test/gtest_factor_absolute.cpp
@@ -17,18 +17,18 @@ using namespace wolf;
using std::cout;
using std::endl;
-Vector10s pose9toPose10(Vector9s _pose9)
+Vector10d pose9toPose10(Vector9d _pose9)
{
- return (Vector10s() << _pose9.head<3>() , v2q(_pose9.segment<3>(3)).coeffs(), _pose9.tail<3>()).finished();
+ return (Vector10d() << _pose9.head<3>() , v2q(_pose9.segment<3>(3)).coeffs(), _pose9.tail<3>()).finished();
}
// Input pose9 and covariance
-Vector9s pose(Vector9s::Random());
-Vector10s pose10 = pose9toPose10(pose);
+Vector9d pose(Vector9d::Random());
+Vector10d pose10 = pose9toPose10(pose);
Eigen::Matrix<double, 9, 9> data_cov = 0.2 * Eigen::Matrix<double,9,9>::Identity();
// perturbated priors
-Vector10s x0 = pose9toPose10(pose + Vector9s::Random()*0.25);
+Vector10d x0 = pose9toPose10(pose + Vector9d::Random()*0.25);
// Problem and solver
ProblemPtr problem_ptr = Problem::create("POV", 3);
diff --git a/test/gtest_factor_autodiff.cpp b/test/gtest_factor_autodiff.cpp
index 6e0ecb764e5e34f81b0a663ca1097beb5d3e0332..bec954adbd00091690b6d8b12a2b9b462d2afb69 100644
--- a/test/gtest_factor_autodiff.cpp
+++ b/test/gtest_factor_autodiff.cpp
@@ -26,13 +26,13 @@ TEST(CaptureAutodiff, ConstructorOdom2d)
IntrinsicsOdom2D intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 0.1;
intrinsics_odo.k_rot_to_rot = 0.1;
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
+ SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3d::Zero(), intrinsics_odo);
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, Eigen::Vector3s::Zero(), Eigen::Matrix3s::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity());
// FACTOR
auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
@@ -46,7 +46,7 @@ TEST(CaptureAutodiff, ConstructorOdom2d)
TEST(CaptureAutodiff, ResidualOdom2d)
{
- Eigen::Vector3s f1_pose, f2_pose;
+ Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
@@ -57,31 +57,31 @@ TEST(CaptureAutodiff, ResidualOdom2d)
IntrinsicsOdom2D intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 0.1;
intrinsics_odo.k_rot_to_rot = 0.1;
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
+ SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3d::Zero(), intrinsics_odo);
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- Eigen::Vector3s d;
+ Eigen::Vector3d d;
d << -1, -4, M_PI/2;
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
// EVALUATE
- Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState();
- Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
- Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
- Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
+ Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
+ Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
+ Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
+ Eigen::VectorXd fr2_ostate = fr2_ptr->getO()->getState();
std::vector<double*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
double const* const* parameters = states_ptr.data();
- Eigen::VectorXs residuals(factor_ptr->getSize());
+ Eigen::VectorXd residuals(factor_ptr->getSize());
factor_ptr->evaluate(parameters, residuals.data(), nullptr);
ASSERT_TRUE(residuals.maxCoeff() < 1e-9);
@@ -90,7 +90,7 @@ TEST(CaptureAutodiff, ResidualOdom2d)
TEST(CaptureAutodiff, JacobianOdom2d)
{
- Eigen::Vector3s f1_pose, f2_pose;
+ Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
@@ -101,57 +101,57 @@ TEST(CaptureAutodiff, JacobianOdom2d)
IntrinsicsOdom2D intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 0.1;
intrinsics_odo.k_rot_to_rot = 0.1;
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
+ SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3d::Zero(), intrinsics_odo);
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- Eigen::Vector3s d;
+ Eigen::Vector3d d;
d << -1, -4, M_PI/2;
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
auto factor_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
// COMPUTE JACOBIANS
- const Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState();
- const Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
- const Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
- const Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
+ const Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
+ const Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
+ const Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
+ const Eigen::VectorXd fr2_ostate = fr2_ptr->getO()->getState();
std::vector<const double*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
- std::vector<Eigen::MatrixXs> Jauto;
- Eigen::VectorXs residuals(factor_ptr->getSize());
+ std::vector<Eigen::MatrixXd> Jauto;
+ Eigen::VectorXd residuals(factor_ptr->getSize());
factor_ptr->evaluate(states_ptr, residuals, Jauto);
std::cout << Jauto.size() << std::endl;
// ANALYTIC JACOBIANS
- Eigen::MatrixXs J0(3,2);
+ Eigen::MatrixXd J0(3,2);
J0 << -cos(f1_pose(2)), -sin(f1_pose(2)),
sin(f1_pose(2)), -cos(f1_pose(2)),
0, 0;
ASSERT_MATRIX_APPROX(J0, Jauto[0], wolf::Constants::EPS);
//ASSERT_TRUE((J0-Jauto[0]).maxCoeff() < 1e-9 && (J0-Jauto[0]).minCoeff() > -1e-9);
- Eigen::MatrixXs J1(3,1);
+ Eigen::MatrixXd J1(3,1);
J1 << -(f2_pose(0) - f1_pose(0)) * sin(f1_pose(2)) + (f2_pose(1) - f1_pose(1)) * cos(f1_pose(2)),
-(f2_pose(0) - f1_pose(0)) * cos(f1_pose(2)) - (f2_pose(1) - f1_pose(1)) * sin(f1_pose(2)),
-1;
ASSERT_MATRIX_APPROX(J1, Jauto[1], wolf::Constants::EPS);
//ASSERT_TRUE((J1-Jauto[1]).maxCoeff() < 1e-9 && (J1-Jauto[1]).minCoeff() > -1e-9);
- Eigen::MatrixXs J2(3,2);
+ Eigen::MatrixXd J2(3,2);
J2 << cos(f1_pose(2)), sin(f1_pose(2)),
-sin(f1_pose(2)), cos(f1_pose(2)),
0, 0;
ASSERT_MATRIX_APPROX(J2, Jauto[2], wolf::Constants::EPS);
//ASSERT_TRUE((J2-Jauto[2]).maxCoeff() < 1e-9 && (J2-Jauto[2]).minCoeff() > -1e-9);
- Eigen::MatrixXs J3(3,1);
+ Eigen::MatrixXd J3(3,1);
J3 << 0, 0, 1;
ASSERT_MATRIX_APPROX(J3, Jauto[3], wolf::Constants::EPS);
//ASSERT_TRUE((J3-Jauto[3]).maxCoeff() < 1e-9 && (J3-Jauto[3]).minCoeff() > -1e-9);
@@ -168,7 +168,7 @@ TEST(CaptureAutodiff, JacobianOdom2d)
TEST(CaptureAutodiff, AutodiffVsAnalytic)
{
- Eigen::Vector3s f1_pose, f2_pose;
+ Eigen::Vector3d f1_pose, f2_pose;
f1_pose << 2,1,M_PI;
f2_pose << 3,5,3*M_PI/2;
@@ -179,15 +179,15 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
IntrinsicsOdom2D intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 0.1;
intrinsics_odo.k_rot_to_rot = 0.1;
- SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3s::Zero(), intrinsics_odo);
+ SensorOdom2DPtr sensor_ptr = std::make_shared<SensorOdom2D>(Vector3d::Zero(), intrinsics_odo);
// CAPTURE
auto capture_ptr = CaptureBase::emplace<CaptureVoid>(fr2_ptr, TimeStamp(0), sensor_ptr);
// FEATURE
- Eigen::Vector3s d;
+ Eigen::Vector3d d;
d << -1, -4, M_PI/2;
- auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3s::Identity());
+ auto feature_ptr = FeatureBase::emplace<FeatureOdom2D>(capture_ptr, d, Eigen::Matrix3d::Identity());
// FACTOR
auto fac_autodiff_ptr = FactorBase::emplace<FactorOdom2D>(feature_ptr, feature_ptr, fr1_ptr);
@@ -195,15 +195,15 @@ TEST(CaptureAutodiff, AutodiffVsAnalytic)
// COMPUTE JACOBIANS
- const Eigen::VectorXs fr1_pstate = fr1_ptr->getP()->getState();
- const Eigen::VectorXs fr1_ostate = fr1_ptr->getO()->getState();
- const Eigen::VectorXs fr2_pstate = fr2_ptr->getP()->getState();
- const Eigen::VectorXs fr2_ostate = fr2_ptr->getO()->getState();
+ const Eigen::VectorXd fr1_pstate = fr1_ptr->getP()->getState();
+ const Eigen::VectorXd fr1_ostate = fr1_ptr->getO()->getState();
+ const Eigen::VectorXd fr2_pstate = fr2_ptr->getP()->getState();
+ const Eigen::VectorXd fr2_ostate = fr2_ptr->getO()->getState();
std::vector<const double*> states_ptr({fr1_pstate.data(), fr1_ostate.data(), fr2_pstate.data(), fr2_ostate.data()});
- std::vector<Eigen::MatrixXs> Jautodiff, Janalytic;
- Eigen::VectorXs residuals(fac_autodiff_ptr->getSize());
+ std::vector<Eigen::MatrixXd> Jautodiff, Janalytic;
+ Eigen::VectorXd residuals(fac_autodiff_ptr->getSize());
clock_t t = clock();
fac_autodiff_ptr->evaluate(states_ptr, residuals, Jautodiff);
std::cout << "autodiff evaluate: " << ((double) clock() - t) / CLOCKS_PER_SEC << "s" << std::endl;
diff --git a/test/gtest_factor_autodiff_distance_3D.cpp b/test/gtest_factor_autodiff_distance_3D.cpp
index e850590b9ea304d13cb6d6259b1e20c6da1efaa5..0e4bf7a2dd6729fee7cdef688b261af3ba3f4a95 100644
--- a/test/gtest_factor_autodiff_distance_3D.cpp
+++ b/test/gtest_factor_autodiff_distance_3D.cpp
@@ -19,19 +19,19 @@ using namespace Eigen;
class FactorAutodiffDistance3D_Test : public testing::Test
{
public:
- Vector3s pos1, pos2;
- Vector3s euler1, euler2;
- Quaternions quat1, quat2;
- Vector4s vquat1, vquat2; // quaternions as vectors
- Vector7s pose1, pose2;
+ Vector3d pos1, pos2;
+ Vector3d euler1, euler2;
+ Quaterniond quat1, quat2;
+ Vector4d vquat1, vquat2; // quaternions as vectors
+ Vector7d pose1, pose2;
FrameBasePtr F1, F2;
CaptureBasePtr C2;
FeatureBasePtr f2;
FactorAutodiffDistance3DPtr c2;
- Vector1s dist;
- Matrix1s dist_cov;
+ Vector1d dist;
+ Matrix1d dist_cov;
ProblemPtr problem;
CeresManagerPtr ceres_manager;
@@ -49,8 +49,8 @@ class FactorAutodiffDistance3D_Test : public testing::Test
pose1 << pos1, vquat1;
pose2 << pos2, vquat2;
- dist = Vector1s(sqrt(2.0));
- dist_cov = Matrix1s(0.01);
+ dist = Vector1d(sqrt(2.0));
+ dist_cov = Matrix1d(0.01);
problem = Problem::create("PO", 3);
ceres_manager = std::make_shared<CeresManager>(problem);
@@ -79,7 +79,7 @@ TEST_F(FactorAutodiffDistance3D_Test, expected_residual)
{
double measurement = 1.400;
- f2->setMeasurement(Vector1s(measurement));
+ f2->setMeasurement(Vector1d(measurement));
double res_expected = (measurement - (pos2-pos1).norm()) * c2->getMeasurementSquareRootInformationUpper()(0,0);
@@ -92,7 +92,7 @@ TEST_F(FactorAutodiffDistance3D_Test, expected_residual)
TEST_F(FactorAutodiffDistance3D_Test, solve)
{
double measurement = 1.400;
- f2->setMeasurement(Vector1s(measurement));
+ f2->setMeasurement(Vector1d(measurement));
std::string report = ceres_manager->solve(SolverManager::ReportVerbosity::QUIET);
diff --git a/test/gtest_factor_diff_drive.cpp b/test/gtest_factor_diff_drive.cpp
index 0d84e6d93baa91096039df40eb69e7399d5be3bb..afad79fe307fcfdc426dcdf9bb6c663e008f3be6 100644
--- a/test/gtest_factor_diff_drive.cpp
+++ b/test/gtest_factor_diff_drive.cpp
@@ -34,44 +34,44 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
{
Base::configure(_sensor);
}
- virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
+ virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
const double _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) const
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const
{
Base::computeCurrentDelta(_data, _data_cov, _calib, _dt, _delta, _delta_cov, _jacobian_calib);
}
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
+ virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
const double _dt2,
- Eigen::VectorXs& _delta1_plus_delta2) const
+ Eigen::VectorXd& _delta1_plus_delta2) const
{
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2);
}
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
+ virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
const double _dt2,
- Eigen::VectorXs& _delta1_plus_delta2,
- Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2) const
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const
{
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
- virtual void statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
+ virtual void statePlusDelta(const Eigen::VectorXd& _x,
+ const Eigen::VectorXd& _delta,
const double _dt,
- Eigen::VectorXs& _x_plus_delta) const
+ Eigen::VectorXd& _x_plus_delta) const
{
Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
}
- virtual Eigen::VectorXs deltaZero() const
+ virtual Eigen::VectorXd deltaZero() const
{
return Base::deltaZero();
}
@@ -79,10 +79,10 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _calib,
- const VectorXs& _calib_preint,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin)
{
return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
@@ -128,12 +128,12 @@ class FactorDiffDriveTest : public testing::Test
FeatureMotionPtr f1;
FactorDiffDrivePtr c1;
- Vector2s data0, data1;
- Matrix2s data_cov;
- Vector3s delta0, delta1;
- Matrix3s delta0_cov, delta1_cov;
- Vector3s calib;
- Vector3s residual;
+ Vector2d data0, data1;
+ Matrix2d data_cov;
+ Vector3d delta0, delta1;
+ Matrix3d delta0_cov, delta1_cov;
+ Vector3d calib;
+ Vector3d residual;
virtual void SetUp()
{
@@ -148,7 +148,7 @@ class FactorDiffDriveTest : public testing::Test
intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!!
intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- Vector3s extr(0, 0, 0);
+ Vector3d extr(0, 0, 0);
auto sen = problem->installSensor("DIFF DRIVE", "sensor", extr, intr);
sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
calib = sensor->getIntrinsic()->getState();
@@ -167,13 +167,13 @@ class FactorDiffDriveTest : public testing::Test
2,
KEY,
0.0,
- Vector3s(0,0,0));
+ Vector3d(0,0,0));
F1 = FrameBase::emplace<FrameBase>(trajectory,
"PO",
2,
KEY,
1.0,
- Vector3s(1,0,0));
+ Vector3d(1,0,0));
// captures
C0 = CaptureBase::emplace<CaptureDiffDrive>(F0,
@@ -202,7 +202,7 @@ class FactorDiffDriveTest : public testing::Test
delta1,
delta1_cov,
C0->getCalibration(),
- Matrix3s::Identity()); // TODO Jacobian?
+ Matrix3d::Identity()); // TODO Jacobian?
// final checks
@@ -225,7 +225,7 @@ class FactorDiffDriveTest : public testing::Test
ASSERT_NE(f1, nullptr);
ASSERT_MATRIX_APPROX(f1->getMeasurement(), delta1, 1e-6);
ASSERT_MATRIX_APPROX(f1->getMeasurementCovariance(), delta1_cov, 1e-6);
- ASSERT_MATRIX_APPROX(f1->getJacobianCalibration(), Matrix3s::Identity(), 1e-6);
+ ASSERT_MATRIX_APPROX(f1->getJacobianCalibration(), Matrix3d::Identity(), 1e-6);
}
};
@@ -266,16 +266,16 @@ TEST_F(FactorDiffDriveTest, residual_zeros)
residual = c1->residual();
- ASSERT_MATRIX_APPROX(residual, Vector3s::Zero(), 1e-12);
+ ASSERT_MATRIX_APPROX(residual, Vector3d::Zero(), 1e-12);
}
TEST_F(FactorDiffDriveTest, residual_right_turn_90_deg)
{
- MatrixXs J_delta_calib(3,3);
+ MatrixXd J_delta_calib(3,3);
// right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
- VectorXs delta(3), delta2(3);
- MatrixXs delta_cov(3,3);
+ VectorXd delta(3), delta2(3);
+ MatrixXd delta_cov(3,3);
double dt = 1.0;
data1(0) = 0.50*intr->ticks_per_wheel_revolution;
@@ -285,26 +285,26 @@ TEST_F(FactorDiffDriveTest, residual_right_turn_90_deg)
delta1 .setZero();
processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
- ASSERT_MATRIX_APPROX(delta2, Vector3s(1.5, -1.5, -M_PI_2), 1e-6);
+ ASSERT_MATRIX_APPROX(delta2, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
f1->setMeasurement(delta2);
- F1->setState(Vector3s(1.5, -1.5, -M_PI_2));
+ F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor);
residual = c1->residual();
- ASSERT_MATRIX_APPROX(residual, Vector3s::Zero(), 1e-12);
+ ASSERT_MATRIX_APPROX(residual, Vector3d::Zero(), 1e-12);
}
TEST_F(FactorDiffDriveTest, solve_F1)
{
- MatrixXs J_delta_calib(3,3);
+ MatrixXd J_delta_calib(3,3);
// right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
- VectorXs delta(3), delta2(3);
- MatrixXs delta_cov(3,3);
+ VectorXd delta(3), delta2(3);
+ MatrixXd delta_cov(3,3);
double dt = 1.0;
data1(0) = 0.50*intr->ticks_per_wheel_revolution;
@@ -315,7 +315,7 @@ TEST_F(FactorDiffDriveTest, solve_F1)
processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
f1->setMeasurement(delta2);
- F1->setState(Vector3s(1.5, -1.5, -M_PI_2));
+ F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor);
@@ -325,7 +325,7 @@ TEST_F(FactorDiffDriveTest, solve_F1)
// Fix all but F1 ; perturb F1 ; solve ; print and assert values of F1
F0->fix();
sensor->fixIntrinsics();
- F1->setState(F1->getState() + Vector3s::Random() * 0.1);
+ F1->setState(F1->getState() + Vector3d::Random() * 0.1);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -338,11 +338,11 @@ TEST_F(FactorDiffDriveTest, solve_F1)
TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
{
- MatrixXs J_delta_calib(3,3);
+ MatrixXd J_delta_calib(3,3);
// right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
- VectorXs delta(3), delta2(3);
- MatrixXs delta_cov(3,3);
+ VectorXd delta(3), delta2(3);
+ MatrixXd delta_cov(3,3);
double dt = 1.0;
data1(0) = 0.50*intr->ticks_per_wheel_revolution;
@@ -353,7 +353,7 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90deg
f1->setMeasurement(delta2);
- F1->setState(Vector3s(1.5, -1.5, -M_PI_2));
+ F1->setState(Vector3d(1.5, -1.5, -M_PI_2));
// wolf: emplace
c1 = FactorBase::emplace<FactorDiffDrive>(f1, f1, C0, processor);
@@ -364,7 +364,7 @@ TEST_F(FactorDiffDriveTest, solve_sensor_intrinsics)
F0->fix();
F1->fix();
sensor->unfixIntrinsics();
- sensor->getIntrinsic()->setState(calib + Vector3s::Random() * 0.1);
+ sensor->getIntrinsic()->setState(calib + Vector3d::Random() * 0.1);
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -422,11 +422,11 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!!
intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- Vector3s extr(0, 0, 0);
+ Vector3d extr(0, 0, 0);
auto sen = problem->installSensor("DIFF DRIVE", "sensor", extr, intr);
auto sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
auto calib_preint = sensor->getIntrinsic()->getState();
- Vector3s calib_gt(1,1,1);
+ Vector3d calib_gt(1,1,1);
// params and processor
ProcessorParamsDiffDrivePtr params = std::make_shared<ProcessorParamsDiffDrive>();
@@ -441,17 +441,17 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
TimeStamp t(0.0);
double dt = 1.0;
- Vector3s x0(0,0,0);
- Vector3s x1(1.5, -1.5, -M_PI/2.0);
- Vector3s x2(3.0, -3.0, 0.0);
- Matrix3s P0; P0.setIdentity();
+ Vector3d x0(0,0,0);
+ Vector3d x1(1.5, -1.5, -M_PI/2.0);
+ Vector3d x2(3.0, -3.0, 0.0);
+ Matrix3d P0; P0.setIdentity();
auto F0 = problem->setPrior(x0, P0, t, 0.1);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
- Vector3s data;
- Matrix2s data_cov; data_cov.setIdentity();
+ Vector3d data;
+ Matrix2d data_cov; data_cov.setIdentity();
data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
@@ -484,7 +484,7 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics_gt)
F2->fix();
// Perturb S
- Vector3s calib_pert = calib_gt + Vector3s::Random()*0.2;
+ Vector3d calib_pert = calib_gt + Vector3d::Random()*0.2;
sensor->getIntrinsic()->setState(calib_pert);
WOLF_TRACE("\n ========== SOLVE =========");
@@ -554,11 +554,11 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
intr->radius_right = 0.98;
intr->wheel_separation = 1.05;
intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- Vector3s extr(0, 0, 0);
+ Vector3d extr(0, 0, 0);
auto sen = problem->installSensor("DIFF DRIVE", "sensor", extr, intr);
auto sensor = std::static_pointer_cast<SensorDiffDrive>(sen);
auto calib_preint = sensor->getIntrinsic()->getState();
- Vector3s calib_gt(1.0, 1.0, 1.0); // ground truth calib
+ Vector3d calib_gt(1.0, 1.0, 1.0); // ground truth calib
// params and processor
ProcessorParamsDiffDrivePtr params = std::make_shared<ProcessorParamsDiffDrive>();
@@ -573,17 +573,17 @@ TEST(FactorDiffDrive, preintegrate_and_solve_sensor_intrinsics)
TimeStamp t(0.0);
double dt = 1.0;
- Vector3s x0(0,0,0);
- Vector3s x1(1.5, -1.5, -M_PI/2.0);
- Vector3s x2(3.0, -3.0, 0.0);
- Matrix3s P0; P0.setIdentity();
+ Vector3d x0(0,0,0);
+ Vector3d x1(1.5, -1.5, -M_PI/2.0);
+ Vector3d x2(3.0, -3.0, 0.0);
+ Matrix3d P0; P0.setIdentity();
auto F0 = problem->setPrior(x0, P0, t, 0.1);
// right turn 90 deg in N steps --> ends up in (1.5, -1.5, -pi/2)
int N = 6;
- Vector3s data;
- Matrix2s data_cov; data_cov.setIdentity();
+ Vector3d data;
+ Matrix2d data_cov; data_cov.setIdentity();
data(0) = 0.50*intr->ticks_per_wheel_revolution / N;
data(1) = 0.25*intr->ticks_per_wheel_revolution / N;
diff --git a/test/gtest_factor_odom_3D.cpp b/test/gtest_factor_odom_3D.cpp
index d6eb4ddf7ea7b07cdb675b96cb7af81e3adc3c8d..2190f048f81625acbd90b7282069b9f216a1404c 100644
--- a/test/gtest_factor_odom_3D.cpp
+++ b/test/gtest_factor_odom_3D.cpp
@@ -17,20 +17,20 @@ using namespace wolf;
using std::cout;
using std::endl;
-Vector7s data2delta(Vector6s _data)
+Vector7d data2delta(Vector6d _data)
{
- return (Vector7s() << _data.head<3>() , v2q(_data.tail<3>()).coeffs()).finished();
+ return (Vector7d() << _data.head<3>() , v2q(_data.tail<3>()).coeffs()).finished();
}
// Input odometry data and covariance
-Vector6s data(Vector6s::Random());
-Vector7s delta = data2delta(data);
-Vector6s data_var((Vector6s() << 0.2,0.2,0.2,0.2,0.2,0.2).finished());
-Matrix6s data_cov = data_var.asDiagonal();
+Vector6d data(Vector6d::Random());
+Vector7d delta = data2delta(data);
+Vector6d data_var((Vector6d() << 0.2,0.2,0.2,0.2,0.2,0.2).finished());
+Matrix6d data_cov = data_var.asDiagonal();
// perturbated priors
-Vector7s x0 = data2delta(Vector6s::Random()*0.25);
-Vector7s x1 = data2delta(data + Vector6s::Random()*0.25);
+Vector7d x0 = data2delta(Vector6d::Random()*0.25);
+Vector7d x1 = data2delta(data + Vector6d::Random()*0.25);
// Problem and solver
ProblemPtr problem_ptr = Problem::create("PO", 3);
diff --git a/test/gtest_factor_pose_2D.cpp b/test/gtest_factor_pose_2D.cpp
index 3c88521c95ab2966b839cd6ab551ef6224b27390..c7c14f80477e8425db6376dda3b01119085f7a59 100644
--- a/test/gtest_factor_pose_2D.cpp
+++ b/test/gtest_factor_pose_2D.cpp
@@ -18,12 +18,12 @@ using std::cout;
using std::endl;
// Input data and covariance
-Vector3s pose(Vector3s::Random());
-Vector3s data_var((Vector3s() << 0.2,0.2,0.2).finished());
-Matrix3s data_cov = data_var.asDiagonal();
+Vector3d pose(Vector3d::Random());
+Vector3d data_var((Vector3d() << 0.2,0.2,0.2).finished());
+Matrix3d data_cov = data_var.asDiagonal();
// perturbated priors
-Vector3s x0 = pose + Vector3s::Random()*0.25;
+Vector3d x0 = pose + Vector3d::Random()*0.25;
// Problem and solver
ProblemPtr problem = Problem::create("PO", 2);
diff --git a/test/gtest_factor_pose_3D.cpp b/test/gtest_factor_pose_3D.cpp
index 0b5032a27978d172432b7f68a04fdea28afc64ff..ff32df29866682d01abc2dd2ce5f7564520b9077 100644
--- a/test/gtest_factor_pose_3D.cpp
+++ b/test/gtest_factor_pose_3D.cpp
@@ -17,19 +17,19 @@ using namespace wolf;
using std::cout;
using std::endl;
-Vector7s pose6toPose7(Vector6s _pose6)
+Vector7d pose6toPose7(Vector6d _pose6)
{
- return (Vector7s() << _pose6.head<3>() , v2q(_pose6.tail<3>()).coeffs()).finished();
+ return (Vector7d() << _pose6.head<3>() , v2q(_pose6.tail<3>()).coeffs()).finished();
}
// Input pose6 and covariance
-Vector6s pose(Vector6s::Random());
-Vector7s pose7 = pose6toPose7(pose);
-Vector6s data_var((Vector6s() << 0.2,0.2,0.2,0.2,0.2,0.2).finished());
-Matrix6s data_cov = data_var.asDiagonal();
+Vector6d pose(Vector6d::Random());
+Vector7d pose7 = pose6toPose7(pose);
+Vector6d data_var((Vector6d() << 0.2,0.2,0.2,0.2,0.2,0.2).finished());
+Matrix6d data_cov = data_var.asDiagonal();
// perturbated priors
-Vector7s x0 = pose6toPose7(pose + Vector6s::Random()*0.25);
+Vector7d x0 = pose6toPose7(pose + Vector6d::Random()*0.25);
// Problem and solver
ProblemPtr problem = Problem::create("PO", 3);
diff --git a/test/gtest_feature_base.cpp b/test/gtest_feature_base.cpp
index 713c044e2684e088606a99b02d3009ce35239f9e..2824fe179096eaca65136b5fadb31b4053bd99cc 100644
--- a/test/gtest_feature_base.cpp
+++ b/test/gtest_feature_base.cpp
@@ -14,12 +14,12 @@ using namespace Eigen;
TEST(FeatureBase, Constructor)
{
- Vector3s m; m << 1,2,3;
- Matrix3s M; M.setRandom(); M = M*M.transpose();
- Matrix3s I = M.inverse();
- Eigen::LLT<Eigen::MatrixXs> llt_of_info(I); // compute the Cholesky decomposition of A
- Eigen::MatrixXs U = llt_of_info.matrixU();
- Eigen::MatrixXs L = llt_of_info.matrixL();
+ Vector3d m; m << 1,2,3;
+ Matrix3d M; M.setRandom(); M = M*M.transpose();
+ Matrix3d I = M.inverse();
+ Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A
+ Eigen::MatrixXd U = llt_of_info.matrixU();
+ Eigen::MatrixXd L = llt_of_info.matrixL();
FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", m, M));
@@ -31,14 +31,14 @@ TEST(FeatureBase, Constructor)
TEST(FeatureBase, SetMeasurement)
{
- Vector3s m; m << 1,2,3;
- Matrix3s M; M.setRandom(); M = M*M.transpose();
- Matrix3s I = M.inverse();
- Eigen::LLT<Eigen::MatrixXs> llt_of_info(I); // compute the Cholesky decomposition of A
- Eigen::MatrixXs U = llt_of_info.matrixU();
- Eigen::MatrixXs L = llt_of_info.matrixL();
+ Vector3d m; m << 1,2,3;
+ Matrix3d M; M.setRandom(); M = M*M.transpose();
+ Matrix3d I = M.inverse();
+ Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A
+ Eigen::MatrixXd U = llt_of_info.matrixU();
+ Eigen::MatrixXd L = llt_of_info.matrixL();
- FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3s::Zero(), Matrix3s::Identity()));
+ FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3d::Zero(), Matrix3d::Identity()));
f->setMeasurement(m);
@@ -47,14 +47,14 @@ TEST(FeatureBase, SetMeasurement)
TEST(FeatureBase, SetMeasurementCovariance)
{
- Vector3s m; m << 1,2,3;
- Matrix3s M; M.setRandom(); M = M*M.transpose();
- Matrix3s I = M.inverse();
- Eigen::LLT<Eigen::MatrixXs> llt_of_info(I); // compute the Cholesky decomposition of A
- Eigen::MatrixXs U = llt_of_info.matrixU();
- Eigen::MatrixXs L = llt_of_info.matrixL();
-
- FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3s::Zero(), Matrix3s::Identity()));
+ Vector3d m; m << 1,2,3;
+ Matrix3d M; M.setRandom(); M = M*M.transpose();
+ Matrix3d I = M.inverse();
+ Eigen::LLT<Eigen::MatrixXd> llt_of_info(I); // compute the Cholesky decomposition of A
+ Eigen::MatrixXd U = llt_of_info.matrixU();
+ Eigen::MatrixXd L = llt_of_info.matrixL();
+
+ FeatureBasePtr f(std::make_shared<FeatureBase>("DUMMY", Vector3d::Zero(), Matrix3d::Identity()));
f->setMeasurementCovariance(M);
diff --git a/test/gtest_frame_base.cpp b/test/gtest_frame_base.cpp
index 2ce7dc51a539ade29c7f640d1d4f105e6115b530..6e47d4903113f4a5e4d80622f51696a123c2cbb1 100644
--- a/test/gtest_frame_base.cpp
+++ b/test/gtest_frame_base.cpp
@@ -53,7 +53,7 @@ TEST(FrameBase, LinksBasic)
ASSERT_FALSE(F->getProblem());
// ASSERT_THROW(f->getPreviousFrame(), std::runtime_error); // protected by assert()
// ASSERT_EQ(f->getStatus(), ST_ESTIMATED); // protected
- SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3s::Zero(), IntrinsicsOdom2D());
+ SensorOdom2DPtr S = make_shared<SensorOdom2D>(Vector3d::Zero(), IntrinsicsOdom2D());
ASSERT_FALSE(F->getCaptureOf(S));
ASSERT_TRUE(F->getCaptureList().empty());
ASSERT_TRUE(F->getConstrainedByList().empty());
@@ -68,13 +68,13 @@ TEST(FrameBase, LinksToTree)
IntrinsicsOdom2D intrinsics_odo;
intrinsics_odo.k_disp_to_disp = 1;
intrinsics_odo.k_rot_to_rot = 1;
- auto S = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Vector3s::Zero(), intrinsics_odo);
+ auto S = SensorBase::emplace<SensorOdom2D>(P->getHardware(), Vector3d::Zero(), intrinsics_odo);
auto F1 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
auto F2 = FrameBase::emplace<FrameBase>(T, 1, make_shared<StateBlock>(2), make_shared<StateBlock>(1));
- auto C = CaptureBase::emplace<CaptureMotion>(F1, "MOTION", 1, S, Vector3s::Zero(), 3, 3, nullptr);
+ auto C = CaptureBase::emplace<CaptureMotion>(F1, "MOTION", 1, S, Vector3d::Zero(), 3, 3, nullptr);
/// @todo link sensor & proccessor
ProcessorBasePtr p = std::make_shared<ProcessorOdom2D>(make_shared<ProcessorParamsOdom2D>());
- auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1s(1), Matrix<double,1,1>::Identity()*.01);
+ auto f = FeatureBase::emplace<FeatureBase>(C, "f", Vector1d(1), Matrix<double,1,1>::Identity()*.01);
auto c = FactorBase::emplace<FactorOdom2D>(f, f, F2, p);
//TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic.
@@ -132,8 +132,8 @@ TEST(FrameBase, GetSetState)
FrameBase F(1, sbp, sbq, sbv);
// Give values to vectors and vector blocks
- VectorXs x(10), x1(10), x2(10);
- VectorXs p(3), v(3), q(4);
+ VectorXd x(10), x1(10), x2(10);
+ VectorXd p(3), v(3), q(4);
p << 1,2,3;
v << 9,8,7;
q << .5, -.5, .5, -.5;
diff --git a/test/gtest_local_param.cpp b/test/gtest_local_param.cpp
index 267d912b6f8d8499b2e956b3bf90f2a5a7b95be5..bcb7d34288912977e6d111acd67380f11c87c655 100644
--- a/test/gtest_local_param.cpp
+++ b/test/gtest_local_param.cpp
@@ -18,10 +18,10 @@
#define JAC_NUMERIC(T, _x0, _J, dx) \
{ \
- VectorXs dv(3); \
- Map<const VectorXs> _dv (dv.data(), 3); \
- VectorXs xo(4); \
- Map<VectorXs> _xo (xo.data(), 4); \
+ VectorXd dv(3); \
+ Map<const VectorXd> _dv (dv.data(), 3); \
+ VectorXd xo(4); \
+ Map<VectorXd> _xo (xo.data(), 4); \
for (int i = 0; i< 3; i++) \
{\
dv.setZero();\
@@ -39,31 +39,31 @@ TEST(TestLocalParametrization, QuaternionLocal)
{
// Storage
- VectorXs x_storage(22);
- MatrixXs M_storage(1,12); // matrix dimensions do not matter, just storage size.
+ VectorXd x_storage(22);
+ MatrixXd M_storage(1,12); // matrix dimensions do not matter, just storage size.
x_storage.setRandom();
M_storage.setZero();
// QUATERNION ------------------------------------------
- Map<VectorXs> q(&x_storage(0),4);
+ Map<VectorXd> q(&x_storage(0),4);
q.normalize();
- Map<VectorXs> da(&x_storage(4),3);
+ Map<VectorXd> da(&x_storage(4),3);
da /= 10.0;
- Map<VectorXs> qo_m(&x_storage(7),4);
- Map<MatrixXs> J(&M_storage(0,0),4,3);
- MatrixXs J_num(4,3);
+ Map<VectorXd> qo_m(&x_storage(7),4);
+ Map<MatrixXd> J(&M_storage(0,0),4,3);
+ MatrixXd J_num(4,3);
LocalParametrizationQuaternion<DQ_LOCAL> Qpar_loc;
- Map<const VectorXs> q_m(q.data(),4);
- Map<const VectorXs> da_m(da.data(),3);
+ Map<const VectorXd> q_m(q.data(),4);
+ Map<const VectorXd> da_m(da.data(),3);
Qpar_loc.plus(q_m,da_m,qo_m);
ASSERT_DOUBLE_EQ(qo_m.norm(), 1);
- Quaternions qref = Map<Quaternions>(q.data()) * wolf::v2q(da);
+ Quaterniond qref = Map<Quaterniond>(q.data()) * wolf::v2q(da);
ASSERT_TRUE(qo_m.isApprox( qref.coeffs() ) );
Qpar_loc.computeJacobian(q_m,J);
@@ -72,8 +72,8 @@ TEST(TestLocalParametrization, QuaternionLocal)
ASSERT_NEAR((J-J_num).norm(), 0, 1e-6);
- Map<const VectorXs> qoc_m(qo_m.data(), 4);
- Map<VectorXs> da2_m(x_storage.data() + 10, 3);
+ Map<const VectorXd> qoc_m(qo_m.data(), 4);
+ Map<VectorXd> da2_m(x_storage.data() + 10, 3);
Qpar_loc.minus(q_m, qoc_m, da2_m);
@@ -84,31 +84,31 @@ TEST(TestLocalParametrization, QuaternionLocal)
TEST(TestLocalParametrization, QuaternionGlobal)
{
// Storage
- VectorXs x_storage(22);
- MatrixXs M_storage(1,12); // matrix dimensions do not matter, just storage size.
+ VectorXd x_storage(22);
+ MatrixXd M_storage(1,12); // matrix dimensions do not matter, just storage size.
x_storage.setRandom();
M_storage.setZero();
// QUATERNION ------------------------------------------
- Map<VectorXs> q(&x_storage(0),4);
+ Map<VectorXd> q(&x_storage(0),4);
q.normalize();
- Map<VectorXs> da(&x_storage(4),3);
+ Map<VectorXd> da(&x_storage(4),3);
da /= 10.0;
- Map<VectorXs> qo_m(&x_storage(7),4);
- Map<MatrixXs> J(&M_storage(0,0),4,3);
- MatrixXs J_num(4,3);
+ Map<VectorXd> qo_m(&x_storage(7),4);
+ Map<MatrixXd> J(&M_storage(0,0),4,3);
+ MatrixXd J_num(4,3);
LocalParametrizationQuaternion<DQ_GLOBAL> Qpar_glob;
- Map<const VectorXs> q_m(q.data(),4);
- Map<const VectorXs> da_m(da.data(),3);
+ Map<const VectorXd> q_m(q.data(),4);
+ Map<const VectorXd> da_m(da.data(),3);
Qpar_glob.plus(q_m,da_m,qo_m);
ASSERT_DOUBLE_EQ(qo_m.norm(), 1);
- Quaternions qref = wolf::v2q(da) * Map<Quaternions>(q.data());
+ Quaterniond qref = wolf::v2q(da) * Map<Quaterniond>(q.data());
ASSERT_TRUE(qo_m.isApprox( qref.coeffs() ) );
Qpar_glob.computeJacobian(q_m,J);
@@ -117,8 +117,8 @@ TEST(TestLocalParametrization, QuaternionGlobal)
ASSERT_NEAR((J-J_num).norm(), 0, 1e-6);
- Map<const VectorXs> qoc_m(qo_m.data(), 4);
- Map<VectorXs> da2_m(x_storage.data() + 10, 3);
+ Map<const VectorXd> qoc_m(qo_m.data(), 4);
+ Map<VectorXd> da2_m(x_storage.data() + 10, 3);
Qpar_glob.minus(q_m, qoc_m, da2_m);
@@ -129,21 +129,21 @@ TEST(TestLocalParametrization, QuaternionGlobal)
TEST(TestLocalParametrization, Homogeneous)
{
// Storage
- VectorXs x_storage(22);
- MatrixXs M_storage(1,12); // matrix dimensions do not matter, just storage size.
+ VectorXd x_storage(22);
+ MatrixXd M_storage(1,12); // matrix dimensions do not matter, just storage size.
// HOMOGENEOUS ----------------------------------------
- Map<VectorXs> h(&x_storage(11),4);
+ Map<VectorXd> h(&x_storage(11),4);
h.setRandom();
- Map<VectorXs> d(&x_storage(15),3);
+ Map<VectorXd> d(&x_storage(15),3);
d << .1,.2,.3;
- Map<VectorXs> ho_m(&x_storage(18),4);
- Map<MatrixXs> J(&M_storage(0,0),4,3);
- MatrixXs J_num(4,3);
+ Map<VectorXd> ho_m(&x_storage(18),4);
+ Map<MatrixXd> J(&M_storage(0,0),4,3);
+ MatrixXd J_num(4,3);
LocalParametrizationHomogeneous Hpar;
- Map<const VectorXs> h_m(h.data(),4);
- Map<const VectorXs> d_m(d.data(),3);
+ Map<const VectorXd> h_m(h.data(),4);
+ Map<const VectorXd> d_m(d.data(),3);
Hpar.plus(h_m,d_m,ho_m);
@@ -155,8 +155,8 @@ TEST(TestLocalParametrization, Homogeneous)
ASSERT_NEAR((J-J_num).norm(), 0, 1e-6);
- Map<const VectorXs> hoc_m(ho_m.data(), 4);
- Map<VectorXs> d2_m(x_storage.data() + 10, 3);
+ Map<const VectorXd> hoc_m(ho_m.data(), 4);
+ Map<VectorXd> d2_m(x_storage.data() + 10, 3);
Hpar.minus(h_m, hoc_m, d2_m);
diff --git a/test/gtest_make_posdef.cpp b/test/gtest_make_posdef.cpp
index 18222fd3b3ec94e009ec649a11730db1a95d4422..607ff99c37d4f0d1670cb617b25cb4b659d777e1 100644
--- a/test/gtest_make_posdef.cpp
+++ b/test/gtest_make_posdef.cpp
@@ -6,7 +6,7 @@ using namespace wolf;
TEST(MakePosDefTest, OkTest)
{
- MatrixXs M = MatrixXs::Identity(3,3);
+ MatrixXd M = MatrixXd::Identity(3,3);
EXPECT_TRUE(isCovariance(M));
EXPECT_FALSE(makePosDef(M));
@@ -14,7 +14,7 @@ TEST(MakePosDefTest, OkTest)
TEST(MakePosDefTest, FixTest)
{
- MatrixXs M = MatrixXs::Zero(3,3);
+ MatrixXd M = MatrixXd::Zero(3,3);
EXPECT_FALSE(isCovariance(M));
EXPECT_TRUE(makePosDef(M));
diff --git a/test/gtest_map_yaml.cpp b/test/gtest_map_yaml.cpp
index 60ac575e2a13591b22a8ce0c0217d049124cc9e2..413a843f8246c982935010c8f7b7d3c0877017ee 100644
--- a/test/gtest_map_yaml.cpp
+++ b/test/gtest_map_yaml.cpp
@@ -22,8 +22,8 @@ TEST(MapYaml, save_2D)
{
ProblemPtr problem = Problem::create("PO", 2);
- Eigen::Vector2s p1, p2, p3;
- Eigen::Vector1s o2, o3;
+ Eigen::Vector2d p1, p2, p3;
+ Eigen::Vector1d o2, o3;
p1 << 1, 2;
p2 << 3, 4;
p3 << 5, 6;
@@ -74,15 +74,15 @@ TEST(MapYaml, load_2D)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() == nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<1,2).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<1,2).finished(), 1e-12);
ASSERT_FALSE(lmk->getP()->isFixed());
}
else if (lmk->id() == 2)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<3,4).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1s()<<2).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<3,4).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<2).finished(), 1e-12);
ASSERT_FALSE(lmk->getP()->isFixed());
ASSERT_FALSE(lmk->getO()->isFixed());
}
@@ -90,8 +90,8 @@ TEST(MapYaml, load_2D)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<5,6).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1s()<<3).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<5,6).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<3).finished(), 1e-12);
ASSERT_TRUE(lmk->getP()->isFixed());
ASSERT_TRUE(lmk->getO()->isFixed());
}
@@ -118,15 +118,15 @@ TEST(MapYaml, load_2D)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() == nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<1,2).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<1,2).finished(), 1e-12);
ASSERT_FALSE(lmk->getP()->isFixed());
}
else if (lmk->id() == 2)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<3,4).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1s()<<2).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<3,4).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<2).finished(), 1e-12);
ASSERT_FALSE(lmk->getP()->isFixed());
ASSERT_FALSE(lmk->getO()->isFixed());
}
@@ -134,8 +134,8 @@ TEST(MapYaml, load_2D)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2s()<<5,6).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1s()<<3).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector2d()<<5,6).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector1d()<<3).finished(), 1e-12);
ASSERT_TRUE(lmk->getP()->isFixed());
ASSERT_TRUE(lmk->getO()->isFixed());
}
@@ -145,8 +145,8 @@ TEST(MapYaml, save_3D)
{
ProblemPtr problem = Problem::create("PO", 3);
- Eigen::Vector3s p1, p2, p3;
- Eigen::Vector4s q2, q3;
+ Eigen::Vector3d p1, p2, p3;
+ Eigen::Vector4d q2, q3;
p1 << 1, 2, 3;
p2 << 4, 5, 6;
p3 << 7, 8, 9;
@@ -197,15 +197,15 @@ TEST(MapYaml, load_3D)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() == nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<1,2,3).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<1,2,3).finished(), 1e-12);
ASSERT_FALSE(lmk->getP()->isFixed());
}
else if (lmk->id() == 2)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<4,5,6).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4s()<<0,1,0,0).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<4,5,6).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,1,0,0).finished(), 1e-12);
ASSERT_FALSE(lmk->getP()->isFixed());
ASSERT_FALSE(lmk->getO()->isFixed());
ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
@@ -214,8 +214,8 @@ TEST(MapYaml, load_3D)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<7,8,9).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4s()<<0,0,1,0).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<7,8,9).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,0,1,0).finished(), 1e-12);
ASSERT_TRUE(lmk->getP()->isFixed());
ASSERT_TRUE(lmk->getO()->isFixed());
ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
@@ -243,15 +243,15 @@ TEST(MapYaml, load_3D)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() == nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<1,2,3).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<1,2,3).finished(), 1e-12);
ASSERT_FALSE(lmk->getP()->isFixed());
}
else if (lmk->id() == 2)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<4,5,6).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4s()<<0,1,0,0).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<4,5,6).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,1,0,0).finished(), 1e-12);
ASSERT_FALSE(lmk->getP()->isFixed());
ASSERT_FALSE(lmk->getO()->isFixed());
ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
@@ -260,8 +260,8 @@ TEST(MapYaml, load_3D)
{
ASSERT_TRUE(lmk->getP() != nullptr);
ASSERT_TRUE(lmk->getO() != nullptr);
- ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3s()<<7,8,9).finished(), 1e-12);
- ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4s()<<0,0,1,0).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getP()->getState(), (Eigen::Vector3d()<<7,8,9).finished(), 1e-12);
+ ASSERT_MATRIX_APPROX(lmk->getO()->getState(), (Eigen::Vector4d()<<0,0,1,0).finished(), 1e-12);
ASSERT_TRUE(lmk->getP()->isFixed());
ASSERT_TRUE(lmk->getO()->isFixed());
ASSERT_TRUE(lmk->getO()->hasLocalParametrization());
diff --git a/test/gtest_motion_buffer.cpp b/test/gtest_motion_buffer.cpp
index 97f639fb8f2db186f1db56d5e6cbcb5e3018e93e..987c713a245df58dba2c32e9edde9505bcd15d56 100644
--- a/test/gtest_motion_buffer.cpp
+++ b/test/gtest_motion_buffer.cpp
@@ -124,7 +124,7 @@ TEST(MotionBuffer, Split)
// MB.get().push_back(m3);
// MB.get().push_back(m4); // put 5 motions
//
-// Eigen::MatrixXs cov = MB.integrateCovariance();
+// Eigen::MatrixXd cov = MB.integrateCovariance();
// ASSERT_NEAR(cov(0), 0.04, 1e-8);
//
// }
@@ -139,7 +139,7 @@ TEST(MotionBuffer, Split)
// MB.get().push_back(m3);
// MB.get().push_back(m4); // put 5 motions
//
-// Eigen::MatrixXs cov = MB.integrateCovariance(t2);
+// Eigen::MatrixXd cov = MB.integrateCovariance(t2);
// ASSERT_NEAR(cov(0), 0.02, 1e-8);
// }
//
@@ -153,7 +153,7 @@ TEST(MotionBuffer, Split)
// MB.get().push_back(m3);
// MB.get().push_back(m4); // put 5 motions
//
-// Eigen::MatrixXs cov = MB.integrateCovariance(t1, t3);
+// Eigen::MatrixXd cov = MB.integrateCovariance(t1, t3);
// ASSERT_NEAR(cov(0), 0.03, 1e-8);
//
// cov = MB.integrateCovariance(t0, t3); // first delta_cov is zero so it does not integrate
diff --git a/test/gtest_odom_2D.cpp b/test/gtest_odom_2D.cpp
index 6e194bc810306c9c149403cbf9da28d448138700..be67f39367ec727944667554e638bd4a7793deab 100644
--- a/test/gtest_odom_2D.cpp
+++ b/test/gtest_odom_2D.cpp
@@ -32,18 +32,18 @@
using namespace wolf;
using namespace Eigen;
-VectorXs plus(const VectorXs& _pose, const Vector2s& _data)
+VectorXd plus(const VectorXd& _pose, const Vector2d& _data)
{
- VectorXs _pose_plus_data(3);
+ VectorXd _pose_plus_data(3);
_pose_plus_data(0) = _pose(0) + cos(_pose(2) + _data(1) / 2) * _data(0);
_pose_plus_data(1) = _pose(1) + sin(_pose(2) + _data(1) / 2) * _data(0);
_pose_plus_data(2) = pi2pi(_pose(2) + _data(1));
return _pose_plus_data;
}
-MatrixXs plus_jac_u(const VectorXs& _pose, const Vector2s& _data)
+MatrixXd plus_jac_u(const VectorXd& _pose, const Vector2d& _data)
{
- MatrixXs Ju(3,2);
+ MatrixXd Ju(3,2);
Ju(0, 0) = cos(_pose(2) + _data(1) / 2);
Ju(0, 1) = -sin(_pose(2) + _data(1) / 2) * _data(0) / 2 ;
Ju(1, 0) = sin(_pose(2) + _data(1) / 2);
@@ -53,9 +53,9 @@ MatrixXs plus_jac_u(const VectorXs& _pose, const Vector2s& _data)
return Ju;
}
-MatrixXs plus_jac_x(const VectorXs& _pose, const Vector2s& _data)
+MatrixXd plus_jac_x(const VectorXd& _pose, const Vector2d& _data)
{
- Matrix3s Jx;
+ Matrix3d Jx;
Jx(0, 0) = 1.0;
Jx(0, 1) = 0.0;
Jx(0, 2) = -sin(_pose(2) + _data(1) / 2) * _data(0);
@@ -93,7 +93,7 @@ void show(const ProblemPtr& problem)
}
}
cout << " state: \n" << F->getState().transpose() << endl;
- Eigen::MatrixXs cov;
+ Eigen::MatrixXd cov;
problem->getFrameCovariance(F,cov);
cout << " covariance: \n" << cov << endl;
}
@@ -119,10 +119,10 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
TimeStamp t0(0.0), t = t0;
double dt = .01;
- Vector3s x0 (0,0,0);
- Eigen::Matrix3s P0 = Eigen::Matrix3s::Identity() * 0.1;
- Vector3s delta (2,0,0);
- Matrix3s delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
+ Vector3d x0 (0,0,0);
+ Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1;
+ Vector3d delta (2,0,0);
+ Matrix3d delta_cov; delta_cov << .02, 0, 0, 0, .025, .02, 0, .02, .02;
ProblemPtr Pr = Problem::create("PO", 2);
CeresManager ceres_manager(Pr);
@@ -132,14 +132,14 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
// KF1 and motion from KF0
t += dt;
- FrameBasePtr F1 = Pr->emplaceFrame(KEY, Vector3s::Zero(), t);
+ FrameBasePtr F1 = Pr->emplaceFrame(KEY, Vector3d::Zero(), t);
auto C1 = CaptureBase::emplace<CaptureBase>(F1, "ODOM 2D", t);
auto f1 = FeatureBase::emplace<FeatureBase>(C1, "ODOM 2D", delta, delta_cov);
auto c1 = FactorBase::emplace<FactorOdom2D>(f1, f1, F0, nullptr);
// KF2 and motion from KF1
t += dt;
- FrameBasePtr F2 = Pr->emplaceFrame(KEY, Vector3s::Zero(), t);
+ FrameBasePtr F2 = Pr->emplaceFrame(KEY, Vector3d::Zero(), t);
auto C2 = CaptureBase::emplace<CaptureBase>(F2, "ODOM 2D", t);
auto f2 = FeatureBase::emplace<FeatureBase>(C2, "ODOM 2D", delta, delta_cov);
auto c2 = FactorBase::emplace<FactorOdom2D>(f2, f2, F1, nullptr);
@@ -147,16 +147,16 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
ASSERT_TRUE(Pr->check(0));
// cout << "===== full ====" << endl;
- F0->setState(Vector3s(1,2,3));
- F1->setState(Vector3s(2,3,1));
- F2->setState(Vector3s(3,1,2));
+ F0->setState(Vector3d(1,2,3));
+ F1->setState(Vector3d(2,3,1));
+ F2->setState(Vector3d(3,1,2));
std::string report = ceres_manager.solve(SolverManager::ReportVerbosity::FULL);
// std::cout << report << std::endl;
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
// show(Pr);
- Matrix3s P1, P2;
+ Matrix3d P1, P2;
P1 << 0.12, 0, 0,
0, 0.525, 0.22,
0, 0.22, 0.12;
@@ -165,16 +165,16 @@ TEST(Odom2D, FactorFix_and_FactorOdom2D)
0, 0.48, 0.14;
// get covariances
- MatrixXs P0_solver, P1_solver, P2_solver;
+ MatrixXd P0_solver, P1_solver, P2_solver;
ASSERT_TRUE(Pr->getFrameCovariance(F0, P0_solver));
ASSERT_TRUE(Pr->getFrameCovariance(F1, P1_solver));
ASSERT_TRUE(Pr->getFrameCovariance(F2, P2_solver));
- ASSERT_POSE2D_APPROX(F0->getState(), Vector3s(0,0,0), 1e-6);
+ ASSERT_POSE2D_APPROX(F0->getState(), Vector3d(0,0,0), 1e-6);
ASSERT_MATRIX_APPROX(P0_solver, P0, 1e-6);
- ASSERT_POSE2D_APPROX(F1->getState(), Vector3s(2,0,0), 1e-6);
+ ASSERT_POSE2D_APPROX(F1->getState(), Vector3d(2,0,0), 1e-6);
ASSERT_MATRIX_APPROX(P1_solver, P1, 1e-6);
- ASSERT_POSE2D_APPROX(F2->getState(), Vector3s(4,0,0), 1e-6);
+ ASSERT_POSE2D_APPROX(F2->getState(), Vector3d(4,0,0), 1e-6);
ASSERT_MATRIX_APPROX(P2_solver, P2, 1e-6);
}
@@ -187,16 +187,16 @@ TEST(Odom2D, VoteForKfAndSolve)
TimeStamp t0(0.0), t = t0;
double dt = .01;
// Origin frame:
- Vector3s x0(0,0,0);
- Eigen::Matrix3s P0 = Eigen::Matrix3s::Identity() * 0.1;
+ Vector3d x0(0,0,0);
+ Eigen::Matrix3d P0 = Eigen::Matrix3d::Identity() * 0.1;
// motion data
- VectorXs data(Vector2s(1, M_PI_4) ); // advance 1m turn pi/4 rad (45 deg). Need 8 steps for a complete turn
- Eigen::MatrixXs data_cov = Eigen::MatrixXs::Identity(2, 2) * 0.01;
+ VectorXd data(Vector2d(1, M_PI_4) ); // advance 1m turn pi/4 rad (45 deg). Need 8 steps for a complete turn
+ Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
int N = 7; // number of process() steps
// Create Wolf tree nodes
ProblemPtr problem = Problem::create("PO", 2);
- SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
+ SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
params->voting_active = true;
params->dist_traveled = 100;
@@ -204,7 +204,7 @@ TEST(Odom2D, VoteForKfAndSolve)
params->max_time_span = 2.5*dt; // force KF at every third process()
params->cov_det = 100;
params->unmeasured_perturbation_std = 0.00;
- Matrix3s unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3s::Identity();
+ Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params);
ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
@@ -226,14 +226,14 @@ TEST(Odom2D, VoteForKfAndSolve)
// std::cout << "Motion data : " << data.transpose() << std::endl;
// Check covariance values
- Eigen::Vector3s integrated_pose = x0;
- Eigen::Matrix3s integrated_cov = P0;
- Eigen::Vector3s integrated_delta = Eigen::Vector3s::Zero();
- Eigen::Matrix3s integrated_delta_cov = Eigen::Matrix3s::Zero();
- Eigen::MatrixXs Ju(3, 2);
- Eigen::Matrix3s Jx;
- std::vector<Eigen::VectorXs> integrated_pose_vector;
- std::vector<Eigen::MatrixXs> integrated_cov_vector;
+ Eigen::Vector3d integrated_pose = x0;
+ Eigen::Matrix3d integrated_cov = P0;
+ Eigen::Vector3d integrated_delta = Eigen::Vector3d::Zero();
+ Eigen::Matrix3d integrated_delta_cov = Eigen::Matrix3d::Zero();
+ Eigen::MatrixXd Ju(3, 2);
+ Eigen::Matrix3d Jx;
+ std::vector<Eigen::VectorXd> integrated_pose_vector;
+ std::vector<Eigen::MatrixXd> integrated_cov_vector;
integrated_pose_vector.push_back(integrated_pose);
integrated_cov_vector.push_back(integrated_cov);
@@ -253,7 +253,7 @@ TEST(Odom2D, VoteForKfAndSolve)
// Processor
capture->process();
ASSERT_TRUE(problem->check(0));
- Matrix3s odom2d_delta_cov = processor_odom2d->getMotion().delta_integr_cov_;
+ Matrix3d odom2d_delta_cov = processor_odom2d->getMotion().delta_integr_cov_;
// Integrate Delta
if (n==3 || n==6) // keyframes
@@ -296,7 +296,7 @@ TEST(Odom2D, VoteForKfAndSolve)
// Check the 3 KFs' states and covariances
- MatrixXs computed_cov;
+ MatrixXd computed_cov;
int n = 0;
for (auto F : problem->getTrajectory()->getFrameList())
{
@@ -320,10 +320,10 @@ TEST(Odom2D, KF_callback)
// time
TimeStamp t0(0.0), t = t0;
double dt = .01;
- Vector3s x0(0,0,0);
- Eigen::Matrix3s x0_cov = Eigen::Matrix3s::Identity() * 0.1;
- VectorXs data(Vector2s(1, M_PI/4) ); // advance 1m
- Eigen::MatrixXs data_cov = Eigen::MatrixXs::Identity(2, 2) * 0.01;
+ Vector3d x0(0,0,0);
+ Eigen::Matrix3d x0_cov = Eigen::Matrix3d::Identity() * 0.1;
+ VectorXd data(Vector2d(1, M_PI/4) ); // advance 1m
+ Eigen::MatrixXd data_cov = Eigen::MatrixXd::Identity(2, 2) * 0.01;
int N = 8; // number of process() steps
// NOTE: We integrate and create KFs as follows:
@@ -336,14 +336,14 @@ TEST(Odom2D, KF_callback)
// Create Wolf tree nodes
ProblemPtr problem = Problem::create("PO", 2);
- SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
+ SensorBasePtr sensor_odom2d = problem->installSensor("ODOM 2D", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
params->dist_traveled = 100;
params->angle_turned = 6.28;
params->max_time_span = 100;
params->cov_det = 100;
params->unmeasured_perturbation_std = 0.000001;
- Matrix3s unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3s::Identity();
+ Matrix3d unmeasured_cov = params->unmeasured_perturbation_std*params->unmeasured_perturbation_std*Matrix3d::Identity();
ProcessorBasePtr prc_base = problem->installProcessor("ODOM 2D", "odom", sensor_odom2d, params);
ProcessorOdom2DPtr processor_odom2d = std::static_pointer_cast<ProcessorOdom2D>(prc_base);
processor_odom2d->setTimeTolerance(dt/2);
@@ -355,14 +355,14 @@ TEST(Odom2D, KF_callback)
FrameBasePtr keyframe_0 = problem->setPrior(x0, x0_cov, t0, dt/2);
// Check covariance values
- Eigen::Vector3s integrated_pose = x0;
- Eigen::Matrix3s integrated_cov = x0_cov;
- Eigen::Vector3s integrated_delta = Eigen::Vector3s::Zero();
- Eigen::Matrix3s integrated_delta_cov = Eigen::Matrix3s::Zero();
- Eigen::MatrixXs Ju(3, 2);
- Eigen::Matrix3s Jx;
- std::vector<Eigen::VectorXs> integrated_pose_vector;
- std::vector<Eigen::MatrixXs> integrated_cov_vector;
+ Eigen::Vector3d integrated_pose = x0;
+ Eigen::Matrix3d integrated_cov = x0_cov;
+ Eigen::Vector3d integrated_delta = Eigen::Vector3d::Zero();
+ Eigen::Matrix3d integrated_delta_cov = Eigen::Matrix3d::Zero();
+ Eigen::MatrixXd Ju(3, 2);
+ Eigen::Matrix3d Jx;
+ std::vector<Eigen::VectorXd> integrated_pose_vector;
+ std::vector<Eigen::MatrixXd> integrated_cov_vector;
// Store integrals
integrated_pose_vector.push_back(integrated_pose);
@@ -412,7 +412,7 @@ TEST(Odom2D, KF_callback)
std::cout << "-----------------------------\nSplit after last KF; time: " << t_split << std::endl;
- Vector3s x_split = processor_odom2d->getState(t_split);
+ Vector3d x_split = processor_odom2d->getState(t_split);
FrameBasePtr keyframe_2 = problem->emplaceFrame(KEY, x_split, t_split);
// there must be 2KF and one F
@@ -437,7 +437,7 @@ TEST(Odom2D, KF_callback)
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState() , integrated_pose_vector[n_split], 1e-6);
- MatrixXs computed_cov;
+ MatrixXd computed_cov;
ASSERT_TRUE(problem->getLastKeyFrameCovariance(computed_cov));
ASSERT_MATRIX_APPROX(computed_cov, integrated_cov_vector [n_split], 1e-6);
@@ -465,9 +465,9 @@ TEST(Odom2D, KF_callback)
// solve
// cout << "===== full ====" << endl;
- keyframe_0->setState(Vector3s(1,2,3));
- keyframe_1->setState(Vector3s(2,3,1));
- keyframe_2->setState(Vector3s(3,1,2));
+ keyframe_0->setState(Vector3d(1,2,3));
+ keyframe_1->setState(Vector3d(2,3,1));
+ keyframe_2->setState(Vector3d(3,1,2));
report = ceres_manager.solve(SolverManager::ReportVerbosity::BRIEF);
ceres_manager.computeCovariances(SolverManager::CovarianceBlocksToBeComputed::ALL_MARGINALS);
@@ -475,13 +475,13 @@ TEST(Odom2D, KF_callback)
problem->print(4,1,1,1);
// check the split KF
- MatrixXs KF1_cov;
+ MatrixXd KF1_cov;
ASSERT_TRUE(problem->getFrameCovariance(keyframe_1, KF1_cov));
ASSERT_POSE2D_APPROX(keyframe_1->getState(), integrated_pose_vector[m_split], 1e-6);
ASSERT_MATRIX_APPROX(KF1_cov, integrated_cov_vector [m_split], 1e-6);
// check other KF in the future of the split KF
- MatrixXs KF2_cov;
+ MatrixXd KF2_cov;
ASSERT_TRUE(problem->getFrameCovariance(keyframe_2, KF2_cov));
ASSERT_POSE2D_APPROX(problem->getLastKeyFrame()->getState(), integrated_pose_vector[n_split], 1e-6);
ASSERT_MATRIX_APPROX(KF2_cov, integrated_cov_vector [n_split], 1e-6);
diff --git a/test/gtest_pack_KF_buffer.cpp b/test/gtest_pack_KF_buffer.cpp
index 6d67622d5648910e4a78b3364516167f2d65e71f..48302e8dd7543a6d20259ddce519800b5d4e9c3d 100644
--- a/test/gtest_pack_KF_buffer.cpp
+++ b/test/gtest_pack_KF_buffer.cpp
@@ -184,7 +184,7 @@ TEST_F(BufferPackKeyFrameTest, selectFirstPackBefore)
// 20.5 null
// 21.5 21
- Eigen::MatrixXs truth(3,6), res(3,6);
+ Eigen::MatrixXd truth(3,6), res(3,6);
truth << 0,10,10,10,10,10, 0,0,0,0,20,20, 0,0,0,0,0,21;
res.setZero();
diff --git a/test/gtest_param_prior.cpp b/test/gtest_param_prior.cpp
index 177961d922ebf117b0a696fd870cfe76f48b86ad..cc5f3a44184ab7da0149bea6e610855de8121349 100644
--- a/test/gtest_param_prior.cpp
+++ b/test/gtest_param_prior.cpp
@@ -18,9 +18,9 @@ using namespace wolf;
ProblemPtr problem_ptr = Problem::create("PO", 3);
CeresManagerPtr ceres_mgr_ptr = std::make_shared<CeresManager>(problem_ptr);
-Eigen::Vector7s initial_extrinsics((Eigen::Vector7s() << 1, 2, 3, 1, 0, 0, 0).finished());
-Eigen::Vector7s prior_extrinsics((Eigen::Vector7s() << 10, 20, 30, 0, 0, 0, 1).finished());
-Eigen::Vector7s prior2_extrinsics((Eigen::Vector7s() << 100, 200, 300, 0, 0, 0, 1).finished());
+Eigen::Vector7d initial_extrinsics((Eigen::Vector7d() << 1, 2, 3, 1, 0, 0, 0).finished());
+Eigen::Vector7d prior_extrinsics((Eigen::Vector7d() << 10, 20, 30, 0, 0, 0, 1).finished());
+Eigen::Vector7d prior2_extrinsics((Eigen::Vector7d() << 100, 200, 300, 0, 0, 0, 1).finished());
SensorOdom3DPtr odom_sensor_ptr_ = std::static_pointer_cast<SensorOdom3D>(problem_ptr->installSensor("ODOM 3D", "odometer", initial_extrinsics, std::make_shared<IntrinsicsOdom3D>()));
@@ -35,7 +35,7 @@ TEST(ParameterPrior, initial_extrinsics)
TEST(ParameterPrior, prior_p)
{
- odom_sensor_ptr_->addPriorP(prior_extrinsics.head(3),Eigen::Matrix3s::Identity());
+ odom_sensor_ptr_->addPriorP(prior_extrinsics.head(3),Eigen::Matrix3d::Identity());
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -44,7 +44,7 @@ TEST(ParameterPrior, prior_p)
TEST(ParameterPrior, prior_o)
{
- odom_sensor_ptr_->addPriorO(prior_extrinsics.tail(4),Eigen::Matrix3s::Identity());
+ odom_sensor_ptr_->addPriorO(prior_extrinsics.tail(4),Eigen::Matrix3d::Identity());
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -53,7 +53,7 @@ TEST(ParameterPrior, prior_o)
TEST(ParameterPrior, prior_p_overwrite)
{
- odom_sensor_ptr_->addPriorP(prior2_extrinsics.head(3),Eigen::Matrix3s::Identity());
+ odom_sensor_ptr_->addPriorP(prior2_extrinsics.head(3),Eigen::Matrix3d::Identity());
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -62,7 +62,7 @@ TEST(ParameterPrior, prior_p_overwrite)
TEST(ParameterPrior, prior_p_segment)
{
- odom_sensor_ptr_->addPriorP(prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2);
+ odom_sensor_ptr_->addPriorP(prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2);
std::string report = ceres_mgr_ptr->solve(SolverManager::ReportVerbosity::BRIEF);
@@ -72,7 +72,7 @@ TEST(ParameterPrior, prior_p_segment)
TEST(ParameterPrior, prior_o_segment)
{
// constraining segment of quaternion is not allowed
- ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter(1, prior_extrinsics.segment(1,2),Eigen::Matrix2s::Identity(),1,2),"");
+ ASSERT_DEATH(odom_sensor_ptr_->addPriorParameter(1, prior_extrinsics.segment(1,2),Eigen::Matrix2d::Identity(),1,2),"");
}
diff --git a/test/gtest_problem.cpp b/test/gtest_problem.cpp
index db68de6ba94f15e26973f3eb97f356cc17a66b7c..4ce4261486328a68b40b927dcb55eee2c89b9d96 100644
--- a/test/gtest_problem.cpp
+++ b/test/gtest_problem.cpp
@@ -92,7 +92,7 @@ TEST(Problem, Processor)
ASSERT_FALSE(P->getProcessorMotion());
// add a motion sensor and processor
- auto Sm = SensorBase::emplace<SensorOdom3D>(P->getHardware(), (Eigen::Vector7s()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D());
+ auto Sm = SensorBase::emplace<SensorOdom3D>(P->getHardware(), (Eigen::Vector7d()<<0,0,0, 0,0,0,1).finished(), IntrinsicsOdom3D());
// add motion processor
auto Pm = ProcessorBase::emplace<ProcessorOdom3D>(Sm, std::make_shared<ProcessorParamsOdom3D>());
@@ -105,7 +105,7 @@ TEST(Problem, Installers)
{
std::string wolf_root = _WOLF_ROOT_DIR;
ProblemPtr P = Problem::create("PO", 3);
- Eigen::Vector7s xs;
+ Eigen::Vector7d xs;
SensorBasePtr S = P->installSensor ("ODOM 3D", "odometer", xs, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
@@ -129,8 +129,8 @@ TEST(Problem, SetOrigin_PO_2D)
{
ProblemPtr P = Problem::create("PO", 2);
TimeStamp t0(0);
- Eigen::VectorXs x0(3); x0 << 1,2,3;
- Eigen::MatrixXs P0(3,3); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
+ Eigen::VectorXd x0(3); x0 << 1,2,3;
+ Eigen::MatrixXd P0(3,3); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
P->setPrior(x0, P0, t0, 1.0);
@@ -166,8 +166,8 @@ TEST(Problem, SetOrigin_PO_3D)
{
ProblemPtr P = Problem::create("PO", 3);
TimeStamp t0(0);
- Eigen::VectorXs x0(7); x0 << 1,2,3,4,5,6,7;
- Eigen::MatrixXs P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
+ Eigen::VectorXd x0(7); x0 << 1,2,3,4,5,6,7;
+ Eigen::MatrixXd P0(6,6); P0.setIdentity(); P0 *= 0.1; // P0 is 0.1*Id
P->setPrior(x0, P0, t0, 1.0);
@@ -203,9 +203,9 @@ TEST(Problem, emplaceFrame_factory)
{
ProblemPtr P = Problem::create("PO", 2);
- FrameBasePtr f0 = P->emplaceFrame("PO", 2, KEY, VectorXs(3), TimeStamp(0.0));
- FrameBasePtr f1 = P->emplaceFrame("PO", 3, KEY, VectorXs(7), TimeStamp(1.0));
- FrameBasePtr f2 = P->emplaceFrame("POV", 3, KEY, VectorXs(10), TimeStamp(2.0));
+ FrameBasePtr f0 = P->emplaceFrame("PO", 2, KEY, VectorXd(3), TimeStamp(0.0));
+ FrameBasePtr f1 = P->emplaceFrame("PO", 3, KEY, VectorXd(7), TimeStamp(1.0));
+ FrameBasePtr f2 = P->emplaceFrame("POV", 3, KEY, VectorXd(10), TimeStamp(2.0));
// std::cout << "f0: type PO 2D? " << f0->getType() << std::endl;
// std::cout << "f1: type PO 3D? " << f1->getType() << std::endl;
@@ -229,8 +229,8 @@ TEST(Problem, StateBlocks)
std::string wolf_root = _WOLF_ROOT_DIR;
ProblemPtr P = Problem::create("PO", 3);
- Eigen::Vector7s xs3d;
- Eigen::Vector3s xs2d;
+ Eigen::Vector7d xs3d;
+ Eigen::Vector3d xs2d;
// 2 state blocks, fixed
SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer",xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
@@ -284,8 +284,8 @@ TEST(Problem, Covariances)
std::string wolf_root = _WOLF_ROOT_DIR;
ProblemPtr P = Problem::create("PO", 3);
- Eigen::Vector7s xs3d;
- Eigen::Vector3s xs2d;
+ Eigen::Vector7d xs3d;
+ Eigen::Vector3d xs2d;
SensorBasePtr Sm = P->installSensor ("ODOM 3D", "odometer", xs3d, wolf_root + "/test/yaml/sensor_odom_3D.yaml");
SensorBasePtr St = P->installSensor ("ODOM 2D", "other odometer", xs2d, wolf_root + "/test/yaml/sensor_odom_2D.yaml");
@@ -303,17 +303,17 @@ TEST(Problem, Covariances)
FrameBasePtr F = P->emplaceFrame("PO", 3, KEY, xs3d, 0);
// set covariance (they are not computed without a solver)
- P->addCovarianceBlock(F->getP(), Eigen::Matrix3s::Identity());
- P->addCovarianceBlock(F->getO(), Eigen::Matrix3s::Identity());
- P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix3s::Zero());
+ P->addCovarianceBlock(F->getP(), Eigen::Matrix3d::Identity());
+ P->addCovarianceBlock(F->getO(), Eigen::Matrix3d::Identity());
+ P->addCovarianceBlock(F->getP(), F->getO(), Eigen::Matrix3d::Zero());
// get covariance
- Eigen::MatrixXs Cov;
+ Eigen::MatrixXd Cov;
ASSERT_TRUE(P->getFrameCovariance(F, Cov));
ASSERT_EQ(Cov.cols() , 6);
ASSERT_EQ(Cov.rows() , 6);
- ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix6s::Identity(), 1e-12);
+ ASSERT_MATRIX_APPROX(Cov, Eigen::Matrix6d::Identity(), 1e-12);
}
diff --git a/test/gtest_processor_base.cpp b/test/gtest_processor_base.cpp
index 9eca5e3c43ea525ec1f81e9613aba2263cd08d5b..7a90b5f6a29c211aba7bea1fb6cad3c0e4a2397e 100644
--- a/test/gtest_processor_base.cpp
+++ b/test/gtest_processor_base.cpp
@@ -38,7 +38,7 @@ TEST(ProcessorBase, KeyFrameCallback)
using std::shared_ptr;
using std::make_shared;
using std::static_pointer_cast;
- using Eigen::Vector2s;
+ using Eigen::Vector2d;
std::string wolf_root = _WOLF_ROOT_DIR;
@@ -50,13 +50,13 @@ TEST(ProcessorBase, KeyFrameCallback)
// Install tracker (sensor and processor)
auto sens_trk = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SENSOR BASE",
- std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
- std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
- std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
+ std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)),
+ std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)),
+ std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
auto proc_trk = problem->installProcessor("TRACKER FEATURE DUMMY", "dummy", sens_trk);
// Install odometer (sensor and processor)
- SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
+ SensorBasePtr sens_odo = problem->installSensor("ODOM 2D", "odometer", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml");
ProcessorParamsOdom2DPtr proc_odo_params = make_shared<ProcessorParamsOdom2D>();
proc_odo_params->time_tolerance = dt/2;
ProcessorBasePtr proc_odo = problem->installProcessor("ODOM 2D", "odom processor", sens_odo, proc_odo_params);
@@ -67,11 +67,11 @@ TEST(ProcessorBase, KeyFrameCallback)
// initialize
TimeStamp t(0.0);
- Vector3s x(0,0,0);
- Matrix3s P = Matrix3s::Identity() * 0.1;
+ Vector3d x(0,0,0);
+ Matrix3d P = Matrix3d::Identity() * 0.1;
problem->setPrior(x, P, t, dt/2); // KF1
- CaptureOdom2DPtr capt_odo = make_shared<CaptureOdom2D>(t, sens_odo, Vector2s(0.5,0));
+ CaptureOdom2DPtr capt_odo = make_shared<CaptureOdom2D>(t, sens_odo, Vector2d(0.5,0));
// Track
CaptureVoidPtr capt_trk(make_shared<CaptureVoid>(t, sens_trk));
diff --git a/test/gtest_processor_diff_drive.cpp b/test/gtest_processor_diff_drive.cpp
index fb358fbafaf332352c6a70701973fae62290576b..86910d63317bddaab485d77fec23a98ad887ca5e 100644
--- a/test/gtest_processor_diff_drive.cpp
+++ b/test/gtest_processor_diff_drive.cpp
@@ -29,44 +29,44 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
{
Base::configure(_sensor);
}
- virtual void computeCurrentDelta(const Eigen::VectorXs& _data,
- const Eigen::MatrixXs& _data_cov,
- const Eigen::VectorXs& _calib,
+ virtual void computeCurrentDelta(const Eigen::VectorXd& _data,
+ const Eigen::MatrixXd& _data_cov,
+ const Eigen::VectorXd& _calib,
const double _dt,
- Eigen::VectorXs& _delta,
- Eigen::MatrixXs& _delta_cov,
- Eigen::MatrixXs& _jacobian_calib) const
+ Eigen::VectorXd& _delta,
+ Eigen::MatrixXd& _delta_cov,
+ Eigen::MatrixXd& _jacobian_calib) const
{
Base::computeCurrentDelta(_data, _data_cov, _calib, _dt, _delta, _delta_cov, _jacobian_calib);
}
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
+ virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
const double _dt2,
- Eigen::VectorXs& _delta1_plus_delta2) const
+ Eigen::VectorXd& _delta1_plus_delta2) const
{
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2);
}
- virtual void deltaPlusDelta(const Eigen::VectorXs& _delta1,
- const Eigen::VectorXs& _delta2,
+ virtual void deltaPlusDelta(const Eigen::VectorXd& _delta1,
+ const Eigen::VectorXd& _delta2,
const double _dt2,
- Eigen::VectorXs& _delta1_plus_delta2,
- Eigen::MatrixXs& _jacobian1,
- Eigen::MatrixXs& _jacobian2) const
+ Eigen::VectorXd& _delta1_plus_delta2,
+ Eigen::MatrixXd& _jacobian1,
+ Eigen::MatrixXd& _jacobian2) const
{
Base::deltaPlusDelta(_delta1, _delta2, _dt2, _delta1_plus_delta2, _jacobian1, _jacobian2);
}
- virtual void statePlusDelta(const Eigen::VectorXs& _x,
- const Eigen::VectorXs& _delta,
+ virtual void statePlusDelta(const Eigen::VectorXd& _x,
+ const Eigen::VectorXd& _delta,
const double _dt,
- Eigen::VectorXs& _x_plus_delta) const
+ Eigen::VectorXd& _x_plus_delta) const
{
Base::statePlusDelta(_x, _delta, _dt, _x_plus_delta);
}
- virtual Eigen::VectorXs deltaZero() const
+ virtual Eigen::VectorXd deltaZero() const
{
return Base::deltaZero();
}
@@ -74,10 +74,10 @@ class ProcessorDiffDrivePublic : public ProcessorDiffDrive
virtual CaptureMotionPtr emplaceCapture(const FrameBasePtr& _frame_own,
const SensorBasePtr& _sensor,
const TimeStamp& _ts,
- const VectorXs& _data,
- const MatrixXs& _data_cov,
- const VectorXs& _calib,
- const VectorXs& _calib_preint,
+ const VectorXd& _data,
+ const MatrixXd& _data_cov,
+ const VectorXd& _calib,
+ const VectorXd& _calib_preint,
const CaptureBasePtr& _capture_origin)
{
return Base::emplaceCapture(_frame_own, _sensor, _ts, _data, _data_cov, _calib, _calib_preint, _capture_origin);
@@ -125,7 +125,7 @@ class ProcessorDiffDriveTest : public testing::Test
intr->radius_right = 1.0; // DO NOT MODIFY THESE VALUES !!!
intr->wheel_separation = 1.0; // DO NOT MODIFY THESE VALUES !!!
intr->ticks_per_wheel_revolution = 100; // DO NOT MODIFY THESE VALUES !!!
- Vector3s extr(0,0,0);
+ Vector3d extr(0,0,0);
sensor = std::static_pointer_cast<SensorDiffDrive>(problem->installSensor("DIFF DRIVE", "sensor diff drive", extr, intr));
// params and processor
@@ -160,7 +160,7 @@ TEST(ProcessorDiffDrive, create)
{
// make a sensor first
auto intr = std::make_shared<IntrinsicsDiffDrive>();
- Vector3s extr(1,2,3);
+ Vector3d extr(1,2,3);
auto sen = std::make_shared<SensorDiffDrive>(extr, intr);
// params
@@ -196,40 +196,40 @@ TEST_F(ProcessorDiffDriveTest, configure)
TEST_F(ProcessorDiffDriveTest, computeCurrentDelta)
{
// 1. zero delta
- Vector2s data(0,0);
- Matrix2s data_cov; data_cov . setIdentity();
- Vector3s calib(1,1,1);
+ Vector2d data(0,0);
+ Matrix2d data_cov; data_cov . setIdentity();
+ Vector3d calib(1,1,1);
double dt = 1.0;
- VectorXs delta(3);
- MatrixXs delta_cov(3,3), J_delta_calib(3,3);
+ VectorXd delta(3);
+ MatrixXd delta_cov(3,3), J_delta_calib(3,3);
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
- ASSERT_MATRIX_APPROX(delta, Vector3s::Zero(), 1e-8);
+ ASSERT_MATRIX_APPROX(delta, Vector3d::Zero(), 1e-8);
// 2. left turn 90 deg --> ends up in (1.5, 1.5, pi/2)
data(0) = 0.25*intr->ticks_per_wheel_revolution;
data(1) = 0.50*intr->ticks_per_wheel_revolution;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
- ASSERT_MATRIX_APPROX(delta, Vector3s(1.5, 1.5, M_PI_2), 1e-6);
+ ASSERT_MATRIX_APPROX(delta, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
// 2. right turn 90 deg --> ends up in (1.5, -1.5, -pi/2)
data(0) = 0.50*intr->ticks_per_wheel_revolution;
data(1) = 0.25*intr->ticks_per_wheel_revolution;
processor->computeCurrentDelta(data, data_cov, calib, dt, delta, delta_cov, J_delta_calib);
- ASSERT_MATRIX_APPROX(delta, Vector3s(1.5, -1.5, -M_PI_2), 1e-6);
+ ASSERT_MATRIX_APPROX(delta, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
}
TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
{
- Vector2s data;
- Matrix2s data_cov; data_cov . setIdentity();
- Vector3s calib(1,1,1);
+ Vector2d data;
+ Matrix2d data_cov; data_cov . setIdentity();
+ Vector3d calib(1,1,1);
double dt = 1.0;
- VectorXs delta(3), delta1(3), delta2(3);
- MatrixXs delta_cov(3,3), J_delta_calib(3,3);
+ VectorXd delta(3), delta1(3), delta2(3);
+ MatrixXd delta_cov(3,3), J_delta_calib(3,3);
// 1. left turn 90 deg in 3 steps of 30 deg --> ends up in (1.5, 1.5, pi/2)
data(0) = 0.25*intr->ticks_per_wheel_revolution / 3;
@@ -243,7 +243,7 @@ TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
delta1 = delta2;
processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90
- ASSERT_MATRIX_APPROX(delta2, Vector3s(1.5, 1.5, M_PI_2), 1e-6);
+ ASSERT_MATRIX_APPROX(delta2, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
// 2. right turn 90 deg in 4 steps of 22.5 deg --> ends up in (1.5, -1.5, -pi/2)
data(0) = 0.50*intr->ticks_per_wheel_revolution / 4;
@@ -259,17 +259,17 @@ TEST_F(ProcessorDiffDriveTest, deltaPlusDelta)
delta1 = delta2;
processor->deltaPlusDelta(delta1, delta, dt, delta2); // 90
- ASSERT_MATRIX_APPROX(delta2, Vector3s(1.5, -1.5, -M_PI_2), 1e-6);
+ ASSERT_MATRIX_APPROX(delta2, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
}
TEST_F(ProcessorDiffDriveTest, statePlusDelta)
{
- Vector2s data;
- Matrix2s data_cov; data_cov . setIdentity();
- Vector3s calib(1,1,1);
+ Vector2d data;
+ Matrix2d data_cov; data_cov . setIdentity();
+ Vector3d calib(1,1,1);
double dt = 1.0;
- VectorXs delta(3), x1(3), x2(3);
- MatrixXs delta_cov(3,3), J_delta_calib(3,3);
+ VectorXd delta(3), x1(3), x2(3);
+ MatrixXd delta_cov(3,3), J_delta_calib(3,3);
// 1. left turn 90 deg in 3 steps of 30 deg --> ends up in (1.5, 1.5, pi/2)
data(0) = 0.25*intr->ticks_per_wheel_revolution / 3;
@@ -283,7 +283,7 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
x1 = x2;
processor->statePlusDelta(x1, delta, dt, x2); // 90
- ASSERT_MATRIX_APPROX(x2, Vector3s(1.5, 1.5, M_PI_2), 1e-6);
+ ASSERT_MATRIX_APPROX(x2, Vector3d(1.5, 1.5, M_PI_2), 1e-6);
// 2. right turn 90 deg in 4 steps of 22.5 deg --> ends up in (1.5, -1.5, -pi/2)
data(0) = 0.50*intr->ticks_per_wheel_revolution / 4;
@@ -299,17 +299,17 @@ TEST_F(ProcessorDiffDriveTest, statePlusDelta)
x1 = x2;
processor->statePlusDelta(x1, delta, dt, x2); // 90
- ASSERT_MATRIX_APPROX(x2, Vector3s(1.5, -1.5, -M_PI_2), 1e-6);
+ ASSERT_MATRIX_APPROX(x2, Vector3d(1.5, -1.5, -M_PI_2), 1e-6);
}
TEST_F(ProcessorDiffDriveTest, process)
{
- Vector2s data;
- Matrix2s data_cov; data_cov . setIdentity();
+ Vector2d data;
+ Matrix2d data_cov; data_cov . setIdentity();
TimeStamp t = 0.0;
double dt = 1.0;
- Vector3s x(0,0,0);
- Matrix3s P; P.setIdentity();
+ Vector3d x(0,0,0);
+ Matrix3d P; P.setIdentity();
auto F0 = problem->setPrior(x, P, t, 0.1);
@@ -335,11 +335,11 @@ TEST_F(ProcessorDiffDriveTest, process)
TEST_F(ProcessorDiffDriveTest, linear)
{
- Vector2s data;
- Matrix2s data_cov; data_cov . setIdentity();
+ Vector2d data;
+ Matrix2d data_cov; data_cov . setIdentity();
TimeStamp t = 0.0;
- Vector3s x(0,0,0);
- Matrix3s P; P.setIdentity();
+ Vector3d x(0,0,0);
+ Matrix3d P; P.setIdentity();
auto F0 = problem->setPrior(x, P, t, 0.1);
@@ -355,16 +355,16 @@ TEST_F(ProcessorDiffDriveTest, linear)
// radius is 1.0m, 100 ticks per revolution, so advanced distance is
double distance = 2 * M_PI * 1.0;
- ASSERT_MATRIX_APPROX(processor->getCurrentState(), Vector3s(distance,0,0), 1e-6)
+ ASSERT_MATRIX_APPROX(processor->getCurrentState(), Vector3d(distance,0,0), 1e-6)
}
TEST_F(ProcessorDiffDriveTest, angular)
{
- Vector2s data;
- Matrix2s data_cov; data_cov . setIdentity();
+ Vector2d data;
+ Matrix2d data_cov; data_cov . setIdentity();
TimeStamp t = 0.0;
- Vector3s x(0,0,0);
- Matrix3s P; P.setIdentity();
+ Vector3d x(0,0,0);
+ Matrix3d P; P.setIdentity();
auto F0 = problem->setPrior(x, P, t, 0.1);
@@ -383,7 +383,7 @@ TEST_F(ProcessorDiffDriveTest, angular)
// radius is 1.0m, 100 ticks per revolution, and wheel separation is 1m, so turn angle is
double angle = pi2pi(2 * M_PI * 1.0 / 0.5 / 5);
- ASSERT_MATRIX_APPROX(processor->getCurrentState(), Vector3s(distance,0,angle), 1e-6)
+ ASSERT_MATRIX_APPROX(processor->getCurrentState(), Vector3d(distance,0,angle), 1e-6)
}
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
index 38fd94f82b8845bbe653a52a087be5afb124bebf..01ead38abbb2e208964ca0957625a8dfd9a22eac 100644
--- a/test/gtest_processor_loopclosure.cpp
+++ b/test/gtest_processor_loopclosure.cpp
@@ -55,7 +55,7 @@ TEST(ProcessorLoopClosure, installProcessor)
using std::shared_ptr;
using std::make_shared;
using std::static_pointer_cast;
- using Eigen::Vector2s;
+ using Eigen::Vector2d;
bool factor_created = false;
@@ -68,17 +68,17 @@ TEST(ProcessorLoopClosure, installProcessor)
// Install tracker (sensor and processor)
auto sens_lc = SensorBase::emplace<SensorBase>(problem->getHardware(),
"SENSOR BASE",
- std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)),
- std::make_shared<StateBlock>(Eigen::VectorXs::Zero(1)),
- std::make_shared<StateBlock>(Eigen::VectorXs::Zero(2)), 2);
+ std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)),
+ std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)),
+ std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
ProcessorParamsLoopClosurePtr params = std::make_shared<ProcessorParamsLoopClosure>();
auto proc_lc = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sens_lc, params, factor_created);
std::cout << "sensor & processor created and added to wolf problem" << std::endl;
// initialize
TimeStamp t(0.0);
- Vector3s x(0,0,0);
- Matrix3s P = Matrix3s::Identity() * 0.1;
+ Vector3d x(0,0,0);
+ Matrix3d P = Matrix3d::Identity() * 0.1;
problem->setPrior(x, P, t, dt/2); // KF1
diff --git a/test/gtest_processor_motion.cpp b/test/gtest_processor_motion.cpp
index ead5d141eb78b22626c4a8f86b3f2637df17cea2..7ab053175c70760fd6a6186a692a4ef78e15e27a 100644
--- a/test/gtest_processor_motion.cpp
+++ b/test/gtest_processor_motion.cpp
@@ -48,8 +48,8 @@ class ProcessorMotion_test : public testing::Test{
SensorOdom2DPtr sensor;
ProcessorOdom2DPublicPtr processor;
CaptureMotionPtr capture;
- Vector2s data;
- Matrix2s data_cov;
+ Vector2d data;
+ Matrix2d data_cov;
// ProcessorMotion_test() :
// ProcessorOdom2D(std::make_shared<ProcessorParamsOdom2D>()),
@@ -62,7 +62,7 @@ class ProcessorMotion_test : public testing::Test{
dt = 1.0;
problem = Problem::create("PO", 2);
- sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("ODOM 2D", "odom", Vector3s(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"));
+ sensor = static_pointer_cast<SensorOdom2D>(problem->installSensor("ODOM 2D", "odom", Vector3d(0,0,0), wolf_root + "/test/yaml/sensor_odom_2D.yaml"));
ProcessorParamsOdom2DPtr params(std::make_shared<ProcessorParamsOdom2D>());
params->time_tolerance = 0.25;
params->dist_traveled = 100;
@@ -73,8 +73,8 @@ class ProcessorMotion_test : public testing::Test{
processor = ProcessorBase::emplace<ProcessorOdom2DPublic>(sensor, params);
capture = std::make_shared<CaptureMotion>("ODOM 2D", 0.0, sensor, data, data_cov, 3, 3, nullptr);
- Vector3s x0; x0 << 0, 0, 0;
- Matrix3s P0; P0.setIdentity();
+ Vector3d x0; x0 << 0, 0, 0;
+ Matrix3d P0; P0.setIdentity();
problem->setPrior(x0, P0, 0.0, 0.01);
}
@@ -98,7 +98,7 @@ TEST_F(ProcessorMotion_test, IntegrateStraight)
WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
}
- ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3s()<<9,0,0).finished(), 1e-8);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<9,0,0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, IntegrateCircle)
@@ -117,7 +117,7 @@ TEST_F(ProcessorMotion_test, IntegrateCircle)
WOLF_DEBUG("t: ", t, " x: ", problem->getCurrentState().transpose());
}
- ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3s()<<0,0,0).finished(), 1e-8);
+ ASSERT_MATRIX_APPROX(problem->getCurrentState(), (Vector3d()<<0,0,0).finished(), 1e-8);
}
TEST_F(ProcessorMotion_test, splitBuffer)
diff --git a/test/gtest_processor_odom_3D.cpp b/test/gtest_processor_odom_3D.cpp
index d6b738afdd8ad5fe0af3a11b9a7863bba10baea1..c683105f70502a30fbadf15dbe40203388a4f44b 100644
--- a/test/gtest_processor_odom_3D.cpp
+++ b/test/gtest_processor_odom_3D.cpp
@@ -15,11 +15,11 @@
#include <iostream>
#define JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, dx) \
-{ VectorXs Do(7); \
+{ VectorXd Do(7); \
prc_ptr->deltaPlusDelta(D, d, dt, Do); \
- VectorXs dd(7); \
- VectorXs DD(7); \
- VectorXs DDo(7); \
+ VectorXd dd(7); \
+ VectorXd DD(7); \
+ VectorXd DDo(7); \
for (int i = 0; i< 7; i++) {\
dd = d;\
DD = D; \
@@ -64,15 +64,15 @@ TEST(ProcessorOdom3D, computeCurrentDelta)
ProcessorOdom3DTest prc;
// input data
- Vector6s data; data.setRandom();
+ Vector6d data; data.setRandom();
double dt = 1; // irrelevant, just for the API.
// Build delta from Eigen tools
- Vector3s data_dp = data.head<3>();
- Vector3s data_do = data.tail<3>();
- Vector3s delta_dp = data_dp;
- Quaternions delta_dq = v2q(data_do);
- Vector7s delta;
+ Vector3d data_dp = data.head<3>();
+ Vector3d data_do = data.tail<3>();
+ Vector3d delta_dp = data_dp;
+ Quaterniond delta_dq = v2q(data_do);
+ Vector7d delta;
delta.head<3>() = delta_dp;
delta.tail<4>() = delta_dq.coeffs();
@@ -81,17 +81,17 @@ TEST(ProcessorOdom3D, computeCurrentDelta)
double rot = data_do.norm();
double dvar = prc.dvar_min() + prc.kdd()*disp;
double rvar = prc.rvar_min() + prc.kdr()*disp + prc.krr()*rot;
- Vector6s diag; diag << dvar, dvar, dvar, rvar, rvar, rvar;
- Matrix6s data_cov = diag.asDiagonal();
- Matrix6s delta_cov = data_cov;
+ Vector6d diag; diag << dvar, dvar, dvar, rvar, rvar, rvar;
+ Matrix6d data_cov = diag.asDiagonal();
+ Matrix6d delta_cov = data_cov;
// return values for data2delta()
- VectorXs delta_ret(7);
- MatrixXs delta_cov_ret(6,6);
- MatrixXs jac_delta_calib(6,0);
+ VectorXd delta_ret(7);
+ MatrixXd delta_cov_ret(6,6);
+ MatrixXd jac_delta_calib(6,0);
// call the function under test
- prc.computeCurrentDelta(data, data_cov, VectorXs::Zero(0), dt, delta_ret, delta_cov_ret, jac_delta_calib);
+ prc.computeCurrentDelta(data, data_cov, VectorXd::Zero(0), dt, delta_ret, delta_cov_ret, jac_delta_calib);
ASSERT_MATRIX_APPROX(delta_ret , delta, Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(delta_cov_ret , delta_cov, Constants::EPS_SMALL);
@@ -102,19 +102,19 @@ TEST(ProcessorOdom3D, deltaPlusDelta)
{
ProcessorOdom3DTest prc;
- VectorXs D(7); D.setRandom(); D.tail<4>().normalize();
- VectorXs d(7); d.setRandom(); d *= 1; d.tail<4>().normalize();
+ VectorXd D(7); D.setRandom(); D.tail<4>().normalize();
+ VectorXd d(7); d.setRandom(); d *= 1; d.tail<4>().normalize();
// Integrated delta value to check aginst
// Dp_int <-- Dp + Dq * dp
// Dq_int <-- Dq * dq
- VectorXs D_int_check(7);
- D_int_check.head<3>() = D.head<3>() + Quaternions(D.data()+3) * d.head<3>();
- D_int_check.tail<4>() = (Quaternions(D.data()+3) * Quaternions(d.data()+3)).coeffs();
+ VectorXd D_int_check(7);
+ D_int_check.head<3>() = D.head<3>() + Quaterniond(D.data()+3) * d.head<3>();
+ D_int_check.tail<4>() = (Quaterniond(D.data()+3) * Quaterniond(d.data()+3)).coeffs();
double dt = 1; // dummy, not used in Odom3D
- VectorXs D_int(7);
+ VectorXd D_int(7);
prc.deltaPlusDelta(D, d, dt, D_int);
@@ -127,19 +127,19 @@ TEST(ProcessorOdom3D, deltaPlusDelta_Jac)
{
std::shared_ptr<ProcessorOdom3DTest> prc_ptr = std::make_shared<ProcessorOdom3DTest>();
- VectorXs D(7); D.setRandom(); D.tail<4>().normalize();
- VectorXs d(7); d.setRandom(); d *= 1; d.tail<4>().normalize();
+ VectorXd D(7); D.setRandom(); D.tail<4>().normalize();
+ VectorXd d(7); d.setRandom(); d *= 1; d.tail<4>().normalize();
double dt = 1;
- VectorXs Do(7);
- MatrixXs D_D(6,6);
- MatrixXs D_d(6,6);
+ VectorXd Do(7);
+ MatrixXd D_D(6,6);
+ MatrixXd D_d(6,6);
prc_ptr->deltaPlusDelta(D, d, dt, Do, D_D, D_d);
WOLF_DEBUG("DD:\n ", D_D);
WOLF_DEBUG("Dd:\n ", D_d);
- MatrixXs J_D(7,7), J_d(7,7);
+ MatrixXd J_D(7,7), J_d(7,7);
JAC_NUMERIC(prc_ptr, D, d, dt, J_D, J_d, Constants::EPS);
WOLF_DEBUG("J_D:\n ", J_D);
diff --git a/test/gtest_processor_tracker_feature_dummy.cpp b/test/gtest_processor_tracker_feature_dummy.cpp
index 31dee4670b60e286a55ea315a8a1507338b3022d..c3a12cd342dd53695823f96e6e4fa8ac8c7b51d9 100644
--- a/test/gtest_processor_tracker_feature_dummy.cpp
+++ b/test/gtest_processor_tracker_feature_dummy.cpp
@@ -82,7 +82,7 @@ class ProcessorTrackerFeatureDummyTest : public testing::Test
problem = Problem::create("PO", 2);
// Install camera
- sensor = problem->installSensor("ODOM 2D", "auxiliar sensor", (Eigen::Vector3s() << 0,0,0).finished(), std::make_shared<IntrinsicsBase>());
+ sensor = problem->installSensor("ODOM 2D", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<IntrinsicsBase>());
// Install processor
params = std::make_shared<ProcessorParamsTrackerFeatureDummy>();
@@ -214,11 +214,11 @@ TEST_F(ProcessorTrackerFeatureDummyTest, processKnown)
TEST_F(ProcessorTrackerFeatureDummyTest, emplaceFactor)
{
FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
- Eigen::Vector1s::Ones(),
- Eigen::MatrixXs::Ones(1, 1)));
+ Eigen::Vector1d::Ones(),
+ Eigen::MatrixXd::Ones(1, 1)));
FeatureBasePtr ftr_other(std::make_shared<FeatureBase>("DUMMY FEATURE",
- Eigen::Vector1s::Ones(),
- Eigen::MatrixXs::Ones(1, 1)));
+ Eigen::Vector1d::Ones(),
+ Eigen::MatrixXd::Ones(1, 1)));
FactorBasePtr fac = processor->callEmplaceFactor(ftr, ftr_other);
diff --git a/test/gtest_processor_tracker_landmark_dummy.cpp b/test/gtest_processor_tracker_landmark_dummy.cpp
index 779ac7de285105263ae1214e0e61463e311983e5..4b207d66ee9643eb1773c16ce40c0a017f939134 100644
--- a/test/gtest_processor_tracker_landmark_dummy.cpp
+++ b/test/gtest_processor_tracker_landmark_dummy.cpp
@@ -100,7 +100,7 @@ class ProcessorTrackerLandmarkDummyTest : public testing::Test
problem = Problem::create("PO", 2);
// Install camera
- sensor = problem->installSensor("ODOM 2D", "auxiliar sensor", (Eigen::Vector3s() << 0,0,0).finished(), std::make_shared<IntrinsicsBase>());
+ sensor = problem->installSensor("ODOM 2D", "auxiliar sensor", (Eigen::Vector3d() << 0,0,0).finished(), std::make_shared<IntrinsicsBase>());
// Install processor
params = std::make_shared<ProcessorParamsTrackerLandmarkDummy>();
@@ -258,8 +258,8 @@ TEST_F(ProcessorTrackerLandmarkDummyTest, processKnown)
TEST_F(ProcessorTrackerLandmarkDummyTest, emplaceFactor)
{
FeatureBasePtr ftr(std::make_shared<FeatureBase>("DUMMY FEATURE",
- Eigen::Vector1s::Ones(),
- Eigen::MatrixXs::Ones(1, 1)));
+ Eigen::Vector1d::Ones(),
+ Eigen::MatrixXd::Ones(1, 1)));
LandmarkBasePtr lmk(std::make_shared<LandmarkBase>("BASE",
std::make_shared<StateBlock>(1),
std::make_shared<StateBlock>(1)));
diff --git a/test/gtest_rotation.cpp b/test/gtest_rotation.cpp
index 8c469eb5621f65ea7da07b51c5b63e9ee84d276e..3fa5bbbd0a11645045aa92fc027598220cc6d7b5 100644
--- a/test/gtest_rotation.cpp
+++ b/test/gtest_rotation.cpp
@@ -29,12 +29,12 @@ using namespace Eigen;
TEST(exp_q, unit_norm)
{
- Vector3s v0 = Vector3s::Random();
+ Vector3d v0 = Vector3d::Random();
double scale = 1.0;
for (int i = 0; i < 20; i++)
{
- Vector3s v = v0 * scale;
- Quaternions q = exp_q(v);
+ Vector3d v = v0 * scale;
+ Quaterniond q = exp_q(v);
ASSERT_NEAR(q.norm(), 1.0, 1e-10) << "Failed at scale 1e-" << i << " with angle = " << 2.0*q.vec().norm();
scale /= 10;
}
@@ -54,12 +54,12 @@ TEST(rotations, pi2pi)
TEST(skew, Skew_vee)
{
using namespace wolf;
- Vector3s vec3 = Vector3s::Random();
- Matrix3s skew_mat;
+ Vector3d vec3 = Vector3d::Random();
+ Matrix3d skew_mat;
skew_mat = skew(vec3);
// vee
- Vector3s vec3_bis;
+ Vector3d vec3_bis;
vec3_bis = vee(skew_mat);
ASSERT_TRUE(vec3_bis == vec3);
@@ -71,21 +71,21 @@ TEST(exp_q, v2q_q2v)
//defines scalars
double deg_to_rad = M_PI/180.0;
- Vector4s vec0, vec1;
+ Vector4d vec0, vec1;
//v2q
- Vector3s rot_vector0, rot_vector1;
- rot_vector0 = Vector3s::Random();
+ Vector3d rot_vector0, rot_vector1;
+ rot_vector0 = Vector3d::Random();
rot_vector1 = rot_vector0 * 100 *deg_to_rad; //far from origin
rot_vector0 = rot_vector0*deg_to_rad;
- Quaternions quat0, quat1;
+ Quaterniond quat0, quat1;
quat0 = v2q(rot_vector0);
quat1 = v2q(rot_vector1);
//q2v
- Vector3s quat_to_v0, quat_to_v1;
- VectorXs quat_to_v0x, quat_to_v1x;
+ Vector3d quat_to_v0, quat_to_v1;
+ VectorXd quat_to_v0x, quat_to_v1x;
quat_to_v0 = q2v(quat0);
quat_to_v1 = q2v(quat1);
@@ -106,18 +106,18 @@ TEST(exp_R, v2R_R2v)
//definition
double deg_to_rad = M_PI/180.0;
- Vector3s rot_vector0, rot_vector1;
+ Vector3d rot_vector0, rot_vector1;
- rot_vector0 = Vector3s::Random();
+ rot_vector0 = Vector3d::Random();
rot_vector1 = rot_vector0 * 100 *deg_to_rad; //far from origin
rot_vector0 = rot_vector0*deg_to_rad;
- Matrix3s rot0, rot1;
+ Matrix3d rot0, rot1;
rot0 = v2R(rot_vector0);
rot1 = v2R(rot_vector1);
//R2v
- Vector3s rot0_vec, rot1_vec;
+ Vector3d rot0_vec, rot1_vec;
rot0_vec = R2v(rot0);
rot1_vec = R2v(rot1);
@@ -131,12 +131,12 @@ TEST(log_R, R2v_v2R_limits)
using namespace wolf;
//test 2 : how small can angles in rotation vector be ?
double scale = 1;
- Matrix3s v_to_R, initial_matrix;
- Vector3s R_to_v;
+ Matrix3d v_to_R, initial_matrix;
+ Vector3d R_to_v;
- //Vector3s rv;
+ //Vector3d rv;
for(int i = 0; i<8; i++){
- initial_matrix = v2R(Vector3s::Random().eval() * scale);
+ initial_matrix = v2R(Vector3d::Random().eval() * scale);
R_to_v = R2v(initial_matrix);
v_to_R = v2R(R_to_v);
@@ -151,18 +151,18 @@ TEST(log_R, R2v_v2R_AAlimits)
using namespace wolf;
//let's see how small the angles can be here : limit reached at scale/10 = 1e-16
double scale = 1;
- Matrix3s rotation_mat;
- Vector3s rv;
+ Matrix3d rotation_mat;
+ Vector3d rv;
for(int i = 0; i<8; i++){
- rotation_mat = v2R(Vector3s::Random().eval() * scale);
+ rotation_mat = v2R(Vector3d::Random().eval() * scale);
//rv = R2v(rotation_mat); //decomposing R2v below
AngleAxis<double> aa0 = AngleAxis<double>(rotation_mat);
rv = aa0.axis() * aa0.angle();
//std::cout << "aa0.axis : " << aa0.axis().transpose() << ",\t aa0.angles :" << aa0.angle() <<std::endl;
- ASSERT_FALSE(rv == Vector3s::Zero());
+ ASSERT_FALSE(rv == Vector3d::Zero());
scale = scale*0.1;
}
}
@@ -174,10 +174,10 @@ TEST(exp_q, v2q2R2v)
// testing new path : vector -> quaternion -> matrix -> vector
for(int i = 0; i< 8; i++){
- Vector3s vector_ = Vector3s::Random()*scale;
- Quaternions quat_ = v2q(vector_);
- Matrix3s mat_ = quat_.matrix();
- Vector3s vector_bis = R2v(mat_);
+ Vector3d vector_ = Vector3d::Random()*scale;
+ Quaterniond quat_ = v2q(vector_);
+ Matrix3d mat_ = quat_.matrix();
+ Vector3d vector_bis = R2v(mat_);
ASSERT_MATRIX_APPROX(vector_, vector_bis, wolf::Constants::EPS);
scale = scale*0.1;
@@ -192,11 +192,11 @@ TEST(rotations, AngleAxis_limits)
// Here we try to get the AngleAxis object from a random rotation matrix, then get back to the rotation matrix using AngleAxis::toRotationMatrix()
double scale = 1;
- Matrix3s res, res_i, rotation_mati, rotation_mat;
- Vector3s rv;
+ Matrix3d res, res_i, rotation_mati, rotation_mat;
+ Vector3d rv;
for(int i = 0; i<8; i++){ //FIX ME : Random() will not create a rotation matrix. Then, R2v(Random_matrix()) makes no sense at all.
- rotation_mat = v2R(Vector3s::Random().eval() * scale);
+ rotation_mat = v2R(Vector3d::Random().eval() * scale);
rotation_mati = rotation_mat;
//rv = R2v(rotation_mat); //decomposing R2v below
@@ -238,10 +238,10 @@ TEST(compose, Quat_compos_const_rateOfTurn)
*/
double deg_to_rad = M_PI/180.0;
- Eigen::Matrix3s rot0(Eigen::Matrix3s::Identity());
- Eigen::Quaternions q0, qRot;
+ Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity());
+ Eigen::Quaterniond q0, qRot;
q0.setIdentity();
- Eigen::Vector3s tmp_vec; //will be used to store rate of turn data
+ Eigen::Vector3d tmp_vec; //will be used to store rate of turn data
const unsigned int x_rot_vel = 5; // deg/s
const unsigned int y_rot_vel = 2; // deg/s
@@ -279,7 +279,7 @@ TEST(compose, Quat_compos_const_rateOfTurn)
// Compare results from rotation matrix composition and quaternion composition
qRot = (v2q(R2v(rot0)));
- Eigen::Vector3s final_orientation(q2v(qRot));
+ Eigen::Vector3d final_orientation(q2v(qRot));
ASSERT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() <<
"\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
}
@@ -309,11 +309,11 @@ TEST(compose, Quat_compos_var_rateOfTurn)
*/
double deg_to_rad = M_PI/180.0;
- Eigen::Matrix3s rot0(Eigen::Matrix3s::Identity());
- Eigen::Quaternions q0, qRot;
+ Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity());
+ Eigen::Quaterniond q0, qRot;
q0.setIdentity();
- Eigen::Vector3s tmp_vec; //will be used to store rate of turn data
+ Eigen::Vector3d tmp_vec; //will be used to store rate of turn data
double time = 0;
const unsigned int x_rot_vel = 15; // deg/s
const unsigned int y_rot_vel = 15; // deg/s
@@ -352,7 +352,7 @@ TEST(compose, Quat_compos_var_rateOfTurn)
// Compare results from rotation matrix composition and quaternion composition
qRot = (v2q(R2v(rot0)));
- Eigen::Vector3s final_orientation(q2v(qRot));
+ Eigen::Vector3d final_orientation(q2v(qRot));
EXPECT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() <<
"\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
@@ -386,11 +386,11 @@ TEST(compose, Quat_compos_var_rateOfTurn_diff)
*/
double deg_to_rad = M_PI/180.0;
- Eigen::Matrix3s rot0(Eigen::Matrix3s::Identity());
- Eigen::Quaternions q0, qRot;
+ Eigen::Matrix3d rot0(Eigen::Matrix3d::Identity());
+ Eigen::Quaterniond q0, qRot;
q0.setIdentity();
- Eigen::Vector3s tmp_vec; //will be used to store rate of turn data
+ Eigen::Vector3d tmp_vec; //will be used to store rate of turn data
double time = 0;
const unsigned int x_rot_vel = 1; // deg/s
const unsigned int y_rot_vel = 3; // deg/s
@@ -429,7 +429,7 @@ TEST(compose, Quat_compos_var_rateOfTurn_diff)
// Compare results from rotation matrix composition and quaternion composition
qRot = (v2q(R2v(rot0)));
- Eigen::Vector3s final_orientation(q2v(qRot));
+ Eigen::Vector3d final_orientation(q2v(qRot));
EXPECT_TRUE((final_orientation - wolf::q2v(q0)).isMuchSmallerThan(1,wolf::Constants::EPS)) << "final orientation expected : " << final_orientation.transpose() <<
"\n computed final orientation : " << wolf::q2v(q0).transpose() << std::endl;
@@ -440,14 +440,14 @@ TEST(compose, Quat_compos_var_rateOfTurn_diff)
TEST(Plus, Random)
{
- Quaternions q;
+ Quaterniond q;
q .coeffs().setRandom().normalize();
- Vector3s v;
+ Vector3d v;
v .setRandom();
- Quaternions q2 = q * exp_q(v);
- Quaternions q3 = exp_q(v) * q;
+ Quaterniond q2 = q * exp_q(v);
+ Quaterniond q3 = exp_q(v) * q;
ASSERT_QUATERNION_APPROX(plus(q,v) , q2, 1e-12);
ASSERT_QUATERNION_APPROX(plus_right(q,v), q2, 1e-12);
@@ -457,14 +457,14 @@ TEST(Plus, Random)
TEST(Plus, Identity_plus_small)
{
- Quaternions q;
+ Quaterniond q;
q .setIdentity();
- Vector3s v;
+ Vector3d v;
v .setRandom();
v *= 1e-6;
- Quaternions q2;
+ Quaterniond q2;
q2.w() = 1;
q2.vec() = 0.5*v;
@@ -473,12 +473,12 @@ TEST(Plus, Identity_plus_small)
TEST(Minus_and_diff, Random)
{
- Quaternions q1, q2, qo;
+ Quaterniond q1, q2, qo;
q1 .coeffs().setRandom().normalize();
q2 .coeffs().setRandom().normalize();
- Vector3s vr = log_q(q1.conjugate() * q2);
- Vector3s vl = log_q(q2 * q1.conjugate());
+ Vector3d vr = log_q(q1.conjugate() * q2);
+ Vector3d vl = log_q(q2 * q1.conjugate());
ASSERT_MATRIX_APPROX(minus(q1, q2), vr, 1e-12);
ASSERT_MATRIX_APPROX(diff(q1, q2), vr, 1e-12);
@@ -499,24 +499,24 @@ TEST(Minus_and_diff, Random)
TEST(Jacobians, Jr)
{
- Vector3s theta; theta.setRandom();
- Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4;
+ Vector3d theta; theta.setRandom();
+ Vector3d dtheta; dtheta.setRandom(); dtheta *= 1e-4;
// Check the main Jr property for q and R
// exp( theta + d_theta ) \approx exp(theta) * exp(Jr * d_theta)
- Matrix3s Jr = jac_SO3_right(theta);
+ Matrix3d Jr = jac_SO3_right(theta);
ASSERT_QUATERNION_APPROX(exp_q(theta+dtheta), exp_q(theta) * exp_q(Jr*dtheta), 1e-8);
ASSERT_MATRIX_APPROX(exp_R(theta+dtheta), (exp_R(theta) * exp_R(Jr*dtheta)), 1e-8);
}
TEST(Jacobians, Jl)
{
- Vector3s theta; theta.setRandom();
- Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4;
+ Vector3d theta; theta.setRandom();
+ Vector3d dtheta; dtheta.setRandom(); dtheta *= 1e-4;
// Check the main Jl property for q and R
// exp( theta + d_theta ) \approx exp(Jl * d_theta) * exp(theta)
- Matrix3s Jl = jac_SO3_left(theta);
+ Matrix3d Jl = jac_SO3_left(theta);
ASSERT_QUATERNION_APPROX(exp_q(theta+dtheta), exp_q(Jl*dtheta) * exp_q(theta), 1e-8);
ASSERT_MATRIX_APPROX(exp_R(theta+dtheta), (exp_R(Jl*dtheta) * exp_R(theta)), 1e-8);
@@ -529,28 +529,28 @@ TEST(Jacobians, Jl)
TEST(Jacobians, Jr_inv)
{
- Vector3s theta; theta.setRandom();
- Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4;
- Quaternions q = v2q(theta);
- Matrix3s R = v2R(theta);
+ Vector3d theta; theta.setRandom();
+ Vector3d dtheta; dtheta.setRandom(); dtheta *= 1e-4;
+ Quaterniond q = v2q(theta);
+ Matrix3d R = v2R(theta);
// Check the main Jr_inv property for q and R
// log( R * exp(d_theta) ) \approx log( R ) + Jrinv * d_theta
- Matrix3s Jr_inv = jac_SO3_right_inv(theta);
+ Matrix3d Jr_inv = jac_SO3_right_inv(theta);
ASSERT_MATRIX_APPROX(log_q(q * exp_q(dtheta)), log_q(q) + Jr_inv*dtheta, 1e-8);
ASSERT_MATRIX_APPROX(log_R(R * exp_R(dtheta)), log_R(R) + Jr_inv*dtheta, 1e-8);
}
TEST(Jacobians, Jl_inv)
{
- Vector3s theta; theta.setRandom();
- Vector3s dtheta; dtheta.setRandom(); dtheta *= 1e-4;
- Quaternions q = v2q(theta);
- Matrix3s R = v2R(theta);
+ Vector3d theta; theta.setRandom();
+ Vector3d dtheta; dtheta.setRandom(); dtheta *= 1e-4;
+ Quaterniond q = v2q(theta);
+ Matrix3d R = v2R(theta);
// Check the main Jl_inv property for q and R
// log( exp(d_theta) * R ) \approx log( R ) + Jlinv * d_theta
- Matrix3s Jl_inv = jac_SO3_left_inv(theta);
+ Matrix3d Jl_inv = jac_SO3_left_inv(theta);
ASSERT_MATRIX_APPROX(log_q(exp_q(dtheta) * q), log_q(q) + Jl_inv*dtheta, 1e-8);
ASSERT_MATRIX_APPROX(log_R(exp_R(dtheta) * R), log_R(R) + Jl_inv*dtheta, 1e-8);
}
@@ -558,19 +558,19 @@ TEST(Jacobians, Jl_inv)
TEST(Jacobians, compose)
{
- Vector3s th1(.1,.2,.3), th2(.3,.1,.2);
- Quaternions q1(exp_q(th1));
- Quaternions q2(exp_q(th2));
- Quaternions qc;
- Matrix3s J1a, J2a, J1n, J2n;
+ Vector3d th1(.1,.2,.3), th2(.3,.1,.2);
+ Quaterniond q1(exp_q(th1));
+ Quaterniond q2(exp_q(th2));
+ Quaterniond qc;
+ Matrix3d J1a, J2a, J1n, J2n;
// composition and analytic Jacobians
wolf::compose(q1, q2, qc, J1a, J2a);
// Numeric Jacobians
double dx = 1e-6;
- Vector3s pert;
- Quaternions q1_pert, q2_pert, qc_pert;
+ Vector3d pert;
+ Quaterniond q1_pert, q2_pert, qc_pert;
for (int i = 0; i<3; i++)
{
pert.setZero();
@@ -594,19 +594,19 @@ TEST(Jacobians, compose)
TEST(Jacobians, between)
{
- Vector3s th1(.1,.2,.3), th2(.3,.1,.2);
- Quaternions q1(exp_q(th1));
- Quaternions q2(exp_q(th2));
- Quaternions qc;
- Matrix3s J1a, J2a, J1n, J2n;
+ Vector3d th1(.1,.2,.3), th2(.3,.1,.2);
+ Quaterniond q1(exp_q(th1));
+ Quaterniond q2(exp_q(th2));
+ Quaterniond qc;
+ Matrix3d J1a, J2a, J1n, J2n;
// composition and analytic Jacobians
wolf::between(q1, q2, qc, J1a, J2a);
// Numeric Jacobians
double dx = 1e-6;
- Vector3s pert;
- Quaternions q1_pert, q2_pert, qc_pert;
+ Vector3d pert;
+ Quaterniond q1_pert, q2_pert, qc_pert;
for (int i = 0; i<3; i++)
{
pert.setZero();
@@ -629,9 +629,9 @@ TEST(Jacobians, between)
TEST(exp_q, small)
{
- Vector3s u; u.setRandom().normalize();
- Vector3s v;
- Quaternions q;
+ Vector3d u; u.setRandom().normalize();
+ Vector3d v;
+ Quaterniond q;
double scale = 1.0;
for (int i = 0; i<20; i++)
{
@@ -647,20 +647,20 @@ TEST(exp_q, small)
TEST(log_q, double_cover)
{
- Quaternions qp; qp.coeffs().setRandom().normalize();
- Quaternions qn; qn.coeffs() = - qp.coeffs();
+ Quaterniond qp; qp.coeffs().setRandom().normalize();
+ Quaterniond qn; qn.coeffs() = - qp.coeffs();
ASSERT_MATRIX_APPROX(log_q(qp), log_q(qn), 1e-16);
}
TEST(log_q, small)
{
- Vector3s u; u.setRandom().normalize();
+ Vector3d u; u.setRandom().normalize();
double scale = 1.0;
for (int i = 0; i<20; i++)
{
- Vector3s v = u*scale;
- Quaternions q = exp_q(v);
- Vector3s l = log_q(q);
+ Vector3d v = u*scale;
+ Quaterniond q = exp_q(v);
+ Vector3d l = log_q(q);
ASSERT_MATRIX_APPROX(v, l, 1e-10);
@@ -672,12 +672,12 @@ TEST(log_q, small)
//=======
TEST(Conversions, q2R_R2q)
{
- Vector3s v; v.setRandom();
- Quaternions q = v2q(v);
- Matrix3s R = v2R(v);
+ Vector3d v; v.setRandom();
+ Quaterniond q = v2q(v);
+ Matrix3d R = v2R(v);
- Quaternions q_R = R2q(R);
- Quaternions qq_R(R);
+ Quaterniond q_R = R2q(R);
+ Quaterniond qq_R(R);
ASSERT_NEAR(q.norm(), 1, wolf::Constants::EPS);
ASSERT_NEAR(q_R.norm(), 1, wolf::Constants::EPS);
@@ -691,8 +691,8 @@ TEST(Conversions, q2R_R2q)
TEST(Conversions, e2q_q2e)
{
- Vector3s e, eo;
- Quaternions q;
+ Vector3d e, eo;
+ Quaterniond q;
e << 0.1, .2, .3;
@@ -709,9 +709,9 @@ TEST(Conversions, e2q_q2e)
//>>>>>>> master
TEST(Conversions, e2q_q2R_R2e)
{
- Vector3s e, eo;
- Quaternions q;
- Matrix3s R;
+ Vector3d e, eo;
+ Quaterniond q;
+ Matrix3d R;
//<<<<<<< HEAD
// e.setRandom();
@@ -739,8 +739,8 @@ TEST(Conversions, e2q_q2R_R2e)
TEST(Conversions, e2R_R2e)
{
- Vector3s e, eo;
- Matrix3s R;
+ Vector3d e, eo;
+ Matrix3d R;
e << 0.1, 0.2, 0.3;
@@ -752,9 +752,9 @@ TEST(Conversions, e2R_R2e)
TEST(Conversions, e2R_R2q_q2e)
{
- Vector3s e, eo;
- Quaternions q;
- Matrix3s R;
+ Vector3d e, eo;
+ Quaterniond q;
+ Matrix3d R;
//<<<<<<< HEAD
// e.setRandom();
@@ -794,7 +794,7 @@ int main(int argc, char **argv)
- vee -> Skew_vee OK
- v2q v -> v2q -> q2v -> v OK (precision wolf::Constants::EPS)
- Matrix<T, 3, 1> q2v(const Quaternion<T>& _q) v -> v2q -> q2v -> v OK (precision wolf::Constants::EPS)
- - VectorXs q2v(const Quaternions& _q) v -> v2q -> q2v -> v OK (precision wolf::Constants::EPS)
+ - VectorXd q2v(const Quaterniond& _q) v -> v2q -> q2v -> v OK (precision wolf::Constants::EPS)
- v2R
- R2v
- jac_SO3_right
diff --git a/test/gtest_sensor_base.cpp b/test/gtest_sensor_base.cpp
index 499629f8fc934eaa5853670dd7d24a69fdc8885c..6ff84c1fd1809694af0bb56925e3dad72a4ca4a2 100644
--- a/test/gtest_sensor_base.cpp
+++ b/test/gtest_sensor_base.cpp
@@ -15,8 +15,8 @@ TEST(SensorBase, setNoiseStd)
{
SensorBasePtr S (std::make_shared<SensorBase>("BASE", nullptr, nullptr, nullptr, 2)); // noise size 2
- Eigen::Vector2s noise_std = Eigen::Vector2s::Ones() * 0.1;
- Eigen::Matrix2s noise_cov = Eigen::Matrix2s::Identity() * 0.01;
+ Eigen::Vector2d noise_std = Eigen::Vector2d::Ones() * 0.1;
+ Eigen::Matrix2d noise_cov = Eigen::Matrix2d::Identity() * 0.01;
S->setNoiseStd(noise_std);
diff --git a/test/gtest_sensor_diff_drive.cpp b/test/gtest_sensor_diff_drive.cpp
index 363847f6c30ea0bcb36fce264f7aabcfe36fc298..3e5d696581c0c5700ff5e43bd837ba634e68b287 100644
--- a/test/gtest_sensor_diff_drive.cpp
+++ b/test/gtest_sensor_diff_drive.cpp
@@ -17,14 +17,14 @@ TEST(DiffDrive, constructor)
{
auto intr = std::make_shared<IntrinsicsDiffDrive>();
- Vector3s extr(1,2,3);
+ Vector3d extr(1,2,3);
auto sen = std::make_shared<SensorDiffDrive>(extr, intr);
ASSERT_NE(sen, nullptr);
- ASSERT_MATRIX_APPROX(sen->getP()->getState(), Vector2s(1,2), 1e-12);
- ASSERT_MATRIX_APPROX(sen->getO()->getState(), Vector1s(3), 1e-12);
+ ASSERT_MATRIX_APPROX(sen->getP()->getState(), Vector2d(1,2), 1e-12);
+ ASSERT_MATRIX_APPROX(sen->getO()->getState(), Vector1d(3), 1e-12);
}
TEST(DiffDrive, getParams)
@@ -35,7 +35,7 @@ TEST(DiffDrive, getParams)
intr->ticks_per_wheel_revolution = 4;
intr->wheel_separation = 5;
- Vector3s extr(1,2,3);
+ Vector3d extr(1,2,3);
auto sen = std::make_shared<SensorDiffDrive>(extr, intr);
@@ -56,7 +56,7 @@ TEST(DiffDrive, create)
intr->ticks_per_wheel_revolution = 4;
intr->wheel_separation = 5;
- Vector3s extr(1,2,3);
+ Vector3d extr(1,2,3);
auto sen_base = SensorDiffDrive::create("name", extr, intr);
diff --git a/test/gtest_solver_manager.cpp b/test/gtest_solver_manager.cpp
index 9b508508932f4112b27a84fb08ca6f9f513b83d1..e67264dce6ad4e433d96fa3aadccfaf596d0aa16 100644
--- a/test/gtest_solver_manager.cpp
+++ b/test/gtest_solver_manager.cpp
@@ -122,7 +122,7 @@ TEST(SolverManager, AddStateBlock)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -141,7 +141,7 @@ TEST(SolverManager, DoubleAddStateBlock)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -166,7 +166,7 @@ TEST(SolverManager, UpdateStateBlock)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -214,7 +214,7 @@ TEST(SolverManager, AddUpdateStateBlock)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -252,7 +252,7 @@ TEST(SolverManager, AddUpdateLocalParamStateBlock)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -295,7 +295,7 @@ TEST(SolverManager, AddLocalParamRemoveLocalParamStateBlock)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Local param
@@ -342,7 +342,7 @@ TEST(SolverManager, RemoveStateBlock)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -367,7 +367,7 @@ TEST(SolverManager, AddRemoveStateBlock)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -389,7 +389,7 @@ TEST(SolverManager, RemoveUpdateStateBlock)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add state_block
@@ -408,7 +408,7 @@ TEST(SolverManager, DoubleRemoveStateBlock)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// add stateblock
@@ -433,14 +433,14 @@ TEST(SolverManager, AddUpdatedStateBlock)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Fix
sb_ptr->fix();
// Set State
- Vector2s state_2 = 2*state;
+ Vector2d state_2 = 2*state;
sb_ptr->setState(state_2);
// Check flags have been set true
@@ -511,13 +511,13 @@ TEST(SolverManager, AddFactor)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
// notification
@@ -538,13 +538,13 @@ TEST(SolverManager, RemoveFactor)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
// update solver
@@ -571,14 +571,14 @@ TEST(SolverManager, AddRemoveFactor)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
// notification
@@ -606,14 +606,14 @@ TEST(SolverManager, DoubleRemoveFactor)
SolverManagerWrapperPtr solver_manager_ptr = std::make_shared<SolverManagerWrapper>(P);
// Create State block
- Vector2s state; state << 1, 2;
+ Vector2d state; state << 1, 2;
StateBlockPtr sb_ptr = std::make_shared<StateBlock>(state);
// Create (and add) factor point 2d
FrameBasePtr F = P->emplaceFrame(KEY, P->zeroState(), TimeStamp(0));
auto C = CaptureBase::emplace<CaptureVoid>(F, 0, nullptr);
- auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3s::Zero(), Matrix3s::Identity());
+ auto f = FeatureBase::emplace<FeatureBase>(C, "ODOM 2D", Vector3d::Zero(), Matrix3d::Identity());
FactorPose2DPtr c = FactorBase::emplace<FactorPose2D>(f, f);
// update solver
diff --git a/test/gtest_track_matrix.cpp b/test/gtest_track_matrix.cpp
index 8d7cafc5f699abc2f0fa9d275f57d5b4b3677821..e9d554077993cc279ef35e434e399082f6a84ab2 100644
--- a/test/gtest_track_matrix.cpp
+++ b/test/gtest_track_matrix.cpp
@@ -16,8 +16,8 @@ class TrackMatrixTest : public testing::Test
public:
TrackMatrix track_matrix;
- Eigen::Vector2s m;
- Eigen::Matrix2s m_cov = Eigen::Matrix2s::Identity()*0.01;
+ Eigen::Vector2d m;
+ Eigen::Matrix2d m_cov = Eigen::Matrix2d::Identity()*0.01;
FrameBasePtr F0, F1, F2, F3, F4;
CaptureBasePtr C0, C1, C2, C3, C4;
diff --git a/test/gtest_trajectory.cpp b/test/gtest_trajectory.cpp
index a142366b64dfc9ca5f5957d415234a0605d01f8e..82c65c284c5f06b518943679ac70ae3f2803bc42 100644
--- a/test/gtest_trajectory.cpp
+++ b/test/gtest_trajectory.cpp
@@ -57,10 +57,10 @@ TEST(TrajectoryBase, ClosestKeyFrame)
// 1 2 3 4 time stamps
// --+-----+-----+-----+---> time
- FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3s::Zero(), 1);
- FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3s::Zero(), 2);
- FrameBasePtr F3 = P->emplaceFrame(AUXILIARY, Eigen::Vector3s::Zero(), 3);
- FrameBasePtr F4 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3s::Zero(), 4);
+ FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 1);
+ FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 2);
+ FrameBasePtr F3 = P->emplaceFrame(AUXILIARY, Eigen::Vector3d::Zero(), 3);
+ FrameBasePtr F4 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 4);
FrameBasePtr KF; // closest key-frame queried
@@ -94,9 +94,9 @@ TEST(TrajectoryBase, ClosestKeyOrAuxFrame)
// 1 2 3 time stamps
// --+-----+-----+---> time
- FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3s::Zero(), 1);
- FrameBasePtr F2 = P->emplaceFrame(AUXILIARY, Eigen::Vector3s::Zero(), 2);
- FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3s::Zero(), 3);
+ FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 1);
+ FrameBasePtr F2 = P->emplaceFrame(AUXILIARY, Eigen::Vector3d::Zero(), 2);
+ FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 3);
FrameBasePtr KF; // closest key-frame queried
@@ -133,21 +133,21 @@ TEST(TrajectoryBase, Add_Remove_Frame)
std::cout << __LINE__ << std::endl;
// add F1
- FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3s::Zero(), 1); // 2 non-fixed
+ FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 1); // 2 non-fixed
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 1);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 2);
std::cout << __LINE__ << std::endl;
// add F2
- FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3s::Zero(), 2); // 1 fixed, 1 not
+ FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 2); // 1 fixed, 1 not
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 2);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
std::cout << __LINE__ << std::endl;
// add F3
- FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3s::Zero(), 3);
+ FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 3);
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getFrameList(). size(), (SizeStd) 3);
ASSERT_EQ(P->getStateBlockNotificationMapSize(), (SizeStd) 4);
@@ -203,19 +203,19 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// --+-----+-----+---> time
// add frames and keyframes in random order --> keyframes must be sorted after that
- FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3s::Zero(), 2);
+ FrameBasePtr F2 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 2);
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getLastFrame() ->id(), F2->id());
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3s::Zero(), 3); // non-key-frame
+ FrameBasePtr F3 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 3); // non-key-frame
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3s::Zero(), 1);
+ FrameBasePtr F1 = P->emplaceFrame(KEY, Eigen::Vector3d::Zero(), 1);
if (debug) P->print(2,0,0,0);
ASSERT_EQ(T->getLastFrame() ->id(), F3->id());
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F2->id());
@@ -227,7 +227,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F3->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F3->id());
- auto F4 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3s::Zero(), 1.5);
+ auto F4 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 1.5);
// Trajectory status:
// KF1 KF2 KF3 F4 frames
// 1 2 3 1.5 time stamps
@@ -265,7 +265,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
if (debug) P->print(2,0,1,0);
ASSERT_EQ(T->getFrameList().front()->id(), F4->id());
- auto F5 = P->emplaceFrame(AUXILIARY, Eigen::Vector3s::Zero(), 1.5);
+ auto F5 = P->emplaceFrame(AUXILIARY, Eigen::Vector3d::Zero(), 1.5);
// Trajectory status:
// KF4 KF2 AuxF5 KF3 KF2 frames
// 0.5 1 1.5 3 4 time stamps
@@ -285,7 +285,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- auto F6 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3s::Zero(), 6);
+ auto F6 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 6);
// Trajectory status:
// KF4 KF2 KF3 KF2 AuxF5 F6 frames
// 0.5 1 3 4 5 6 time stamps
@@ -295,7 +295,7 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
ASSERT_EQ(T->getLastKeyOrAuxFrame()->id(), F5->id());
ASSERT_EQ(T->getLastKeyFrame() ->id(), F2->id());
- auto F7 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3s::Zero(), 5.5);
+ auto F7 = P->emplaceFrame(NON_ESTIMATED, Eigen::Vector3d::Zero(), 5.5);
// Trajectory status:
// KF4 KF2 KF3 KF2 AuxF5 F7 F6 frames
// 0.5 1 3 4 5 5.5 6 time stamps
diff --git a/test/gtest_yaml_conversions.cpp b/test/gtest_yaml_conversions.cpp
index 2c7c70b27e429557508474d9484978c53597ca4a..eb42585bd47b6217416d6741a2053bff582f2eda 100644
--- a/test/gtest_yaml_conversions.cpp
+++ b/test/gtest_yaml_conversions.cpp
@@ -18,8 +18,8 @@ using namespace wolf;
TEST(MapYaml, save_2D)
{
- MatrixXs M23(2,3);
- MatrixXs M33(3,3);
+ MatrixXd M23(2,3);
+ MatrixXd M33(3,3);
M23 << 1, 2, 3, 4, 5, 6;
M33 << 1, 2, 3, 4, 5, 6, 7, 8, 9;
@@ -33,7 +33,7 @@ TEST(MapYaml, save_2D)
mat_23 = YAML::Load("[1, 2, 3, 4, 5, 6]"); // insensitive to spacing
mat_33 = YAML::Load("[1, 2, 3, 4, 5, 6, 7, 8, 9]"); // insensitive to spacing
- MatrixXs Mx = mat_sized_23.as<MatrixXs>();
+ MatrixXd Mx = mat_sized_23.as<MatrixXd>();
std::cout << "Dyn-Dyn [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << Mx << std::endl;
ASSERT_MATRIX_APPROX(Mx, M23, 1e-12);
@@ -45,7 +45,7 @@ TEST(MapYaml, save_2D)
std::cout << "Dyn-3 [[2,3] ,[1, 2, 3, 4, 5, 6] ] = \n" << MD3 << std::endl;
ASSERT_MATRIX_APPROX(MD3, M23, 1e-12);
- Matrix3s M3 = mat_sized_33.as<Matrix3s>();
+ Matrix3d M3 = mat_sized_33.as<Matrix3d>();
std::cout << "3-3 [[3,3] ,[1, 2, 3, 4, 5, 6, 7, 8, 9] ] = \n" << M3 << std::endl;
ASSERT_MATRIX_APPROX(M3, M33, 1e-12);
@@ -57,7 +57,7 @@ TEST(MapYaml, save_2D)
std::cout << "Dyn-3 [1, 2, 3, 4, 5, 6] = \n" << MD3 << std::endl;
ASSERT_MATRIX_APPROX(MD3, M23, 1e-12);
- M3 = mat_33.as<Matrix3s>();
+ M3 = mat_33.as<Matrix3d>();
std::cout << "3-3 [1, 2, 3, 4, 5, 6, 7, 8, 9] = \n" << M3 << std::endl;
ASSERT_MATRIX_APPROX(M3, M33, 1e-12);
}