diff --git a/demos/demo_2_lasers_offline.cpp b/demos/demo_2_lasers_offline.cpp
index 58e91c6d8a63e81ff0337ee1ec4a7b601cdb9de7..40ebda489e205e21154a7501f3becacb6b8caced 100644
--- a/demos/demo_2_lasers_offline.cpp
+++ b/demos/demo_2_lasers_offline.cpp
@@ -37,7 +37,7 @@
#include "faramotics/rangeScan2D.h"
#include "btr-headers/pose3d.h"
-void extractVector(std::ifstream& text_file, Eigen::VectorXs& vector, wolf::Scalar& ts)
+void extractVector(std::ifstream& text_file, Eigen::VectorXd& vector, double& ts)
{
std::string line;
std::getline(text_file, line);
@@ -47,7 +47,7 @@ void extractVector(std::ifstream& text_file, Eigen::VectorXs& vector, wolf::Scal
line_stream >> vector(i);
}
-void extractScan(std::ifstream& text_file, std::vector<float>& scan, wolf::Scalar& ts)
+void extractScan(std::ifstream& text_file, std::vector<float>& scan, double& ts)
{
std::string line;
std::getline(text_file, line);
@@ -92,27 +92,27 @@ int main(int argc, char** argv)
// INITIALIZATION ============================================================================================
//init random generators
- Scalar odom_std_factor = 0.5;
+ double odom_std_factor = 0.5;
std::default_random_engine generator(1);
- std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise
+ std::normal_distribution<double> distribution_odom(0.0, odom_std_factor); //odometry noise
//variables
std::string line;
- Eigen::VectorXs odom_data = Eigen::VectorXs::Zero(2);
- Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
- Eigen::VectorXs ground_truth_pose(3); //last true pose
- Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
- Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(6);
+ Eigen::VectorXd odom_data = Eigen::VectorXd::Zero(2);
+ Eigen::VectorXd ground_truth(n_execution * 3); //all true poses
+ Eigen::VectorXd ground_truth_pose(3); //last true pose
+ Eigen::VectorXd odom_trajectory(n_execution * 3); //open loop trajectory
+ Eigen::VectorXd mean_times = Eigen::VectorXd::Zero(6);
clock_t t1, t2;
- Scalar timestamp;
+ double timestamp;
TimeStamp ts(0);
// Wolf initialization
- Eigen::VectorXs odom_pose = Eigen::VectorXs::Zero(3);
- //Eigen::VectorXs gps_position = Eigen::VectorXs::Zero(2);
- Eigen::VectorXs laser_1_params(9), laser_2_params(9);
- Eigen::VectorXs laser_1_pose(4), laser_2_pose(4); //xyz + theta
- Eigen::VectorXs laser_1_pose2D(3), laser_2_pose2D(3); //xy + theta
+ Eigen::VectorXd odom_pose = Eigen::VectorXd::Zero(3);
+ //Eigen::VectorXd gps_position = Eigen::VectorXd::Zero(2);
+ Eigen::VectorXd laser_1_params(9), laser_2_params(9);
+ Eigen::VectorXd laser_1_pose(4), laser_2_pose(4); //xyz + theta
+ Eigen::VectorXd laser_1_pose2D(3), laser_2_pose2D(3); //xy + theta
// odometry intrinsics
IntrinsicsOdom2D odom_intrinsics;
@@ -160,7 +160,7 @@ int main(int argc, char** argv)
std::cout << "Wolf tree setted correctly!" << std::endl;
- CaptureMotion* odom_capture = new CaptureMotion(ts, odom_sensor, odom_data, Eigen::Matrix2s::Identity() * odom_std_factor * odom_std_factor, nullptr);
+ CaptureMotion* odom_capture = new CaptureMotion(ts, odom_sensor, odom_data, Eigen::Matrix2d::Identity() * odom_std_factor * odom_std_factor, nullptr);
// Initial pose
ground_truth_pose << 2, 8, 0;
@@ -168,7 +168,7 @@ int main(int argc, char** argv)
odom_trajectory.head(3) = ground_truth_pose;
// Origin Key Frame with covariance
- problem.setPrior(ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1, ts, 0.1);
+ problem.setPrior(ground_truth_pose, Eigen::Matrix3d::Identity() * 0.1, ts, 0.1);
// Ceres wrapper
ceres::Solver::Options ceres_options;
@@ -262,7 +262,7 @@ int main(int argc, char** argv)
// Print Final result in a file -------------------------
// Vehicle poses
int i = 0;
- Eigen::VectorXs state_poses = Eigen::VectorXs::Zero(n_execution * 3);
+ Eigen::VectorXd state_poses = Eigen::VectorXd::Zero(n_execution * 3);
for (auto frame : *(problem.getTrajectory()->getFrameList()))
{
state_poses.segment(i, 3) << frame->getP()->getVector(), frame->getO()->getVector();
@@ -271,7 +271,7 @@ int main(int argc, char** argv)
// Landmarks
i = 0;
- Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMap()->getLandmarkList()->size() * 2);
+ Eigen::VectorXd landmarks = Eigen::VectorXd::Zero(problem.getMap()->getLandmarkList()->size() * 2);
for (auto landmark : *(problem.getMap()->getLandmarkList()))
{
landmarks.segment(i, 2) = landmark->getP()->getVector();
diff --git a/demos/demo_capture_laser_2D.cpp b/demos/demo_capture_laser_2D.cpp
index cd5e40239fd42ca972a0e96405dc609ab707e9ca..15367a4c13298de7549d4b55613e0d59da124c4a 100644
--- a/demos/demo_capture_laser_2D.cpp
+++ b/demos/demo_capture_laser_2D.cpp
@@ -17,7 +17,7 @@ int main(int argc, char *argv[])
std::cout << "========================================================" << std::endl;
//scan ranges
- Eigen::VectorXs ranges(720);
+ Eigen::VectorXd ranges(720);
ranges << 2.78886,2.78289,2.7832,2.78367,2.7843,2.78508,2.78603,2.78713,2.78839,2.78981,
2.79139,2.78669,2.78855,2.79057,2.79274,2.79507,2.79098,2.79359,2.79635,2.79927,
2.79566,2.79886,2.80221,2.79895,2.80258,2.80638,2.80345,2.80753,2.81176,2.80917,
@@ -90,11 +90,11 @@ int main(int argc, char *argv[])
27.2361,27.3466,27.4586,27.572,27.6869,27.8033,27.9213,28.0407,28.1617;
//variable declarations and inits
- Eigen::VectorXs device_pose(6);
+ Eigen::VectorXd device_pose(6);
device_pose << 0,0,0,0,0,0; //origin, no rotation
TimeStamp time_stamp;
time_stamp.setToNow();
- std::list<Eigen::Vector4s, Eigen::aligned_allocator<Eigen::Vector4s> > corner_list;
+ std::list<Eigen::Vector4d, Eigen::aligned_allocator<Eigen::Vector4d> > corner_list;
//Create Device objects
//SensorLaser2D device(device_pose, ranges.size(), M_PI, 0.2, 30.0, 0.01);
@@ -103,7 +103,7 @@ int main(int argc, char *argv[])
//init a noise generator
std::default_random_engine generator(1);
- std::normal_distribution<Scalar> distribution_range(0.,device.getRangeStdDev()); //odometry noise
+ std::normal_distribution<double> distribution_range(0.,device.getRangeStdDev()); //odometry noise
//Create a Capture object
CaptureLaser2D capture(time_stamp, &device, ranges);
diff --git a/demos/demo_ceres_2_lasers.cpp b/demos/demo_ceres_2_lasers.cpp
index dce55b16f3fd730ddaf8b0832736bb79bd30bccb..050f47f2208da737a3c494697b230786450df7a8 100644
--- a/demos/demo_ceres_2_lasers.cpp
+++ b/demos/demo_ceres_2_lasers.cpp
@@ -48,10 +48,10 @@ class FaramoticsRobot
string modelFileName;
CrangeScan2D* myScanner;
CdynamicSceneRender* myRender;
- Eigen::Vector3s ground_truth_pose_;
- Eigen::Vector4s laser_1_pose_, laser_2_pose_;
+ Eigen::Vector3d ground_truth_pose_;
+ Eigen::Vector4d laser_1_pose_, laser_2_pose_;
- FaramoticsRobot(int argc, char** argv, const Eigen::Vector4s& _laser_1_pose, const Eigen::Vector4s& _laser_2_pose) :
+ FaramoticsRobot(int argc, char** argv, const Eigen::Vector4d& _laser_1_pose, const Eigen::Vector4d& _laser_2_pose) :
modelFileName("/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"),
laser_1_pose_(_laser_1_pose),
laser_2_pose_(_laser_2_pose)
@@ -72,7 +72,7 @@ class FaramoticsRobot
}
//function travel around
- Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
+ Eigen::Vector3d motionCampus(unsigned int ii, double& displacement_, double& rotation_)
{
if (ii <= 120){ displacement_ = 0.1; rotation_ = 0; }
else if (ii <= 170) { displacement_ = 0.2; rotation_ = 1.8 * M_PI / 180; }
@@ -126,7 +126,7 @@ class FaramoticsRobot
}
}
- void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3s& estimated_pose)
+ void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3d& estimated_pose)
{
// detected corners
//std::cout << " drawCorners: " << feature_list.size() << std::endl;
@@ -146,7 +146,7 @@ class FaramoticsRobot
landmark_vector.reserve(3*landmark_list.size());
for (auto landmark : landmark_list)
{
- Scalar* position_ptr = landmark->getP()->get();
+ double* position_ptr = landmark->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -195,27 +195,27 @@ int main(int argc, char** argv)
// INITIALIZATION ============================================================================================
//init random generators
- Scalar odom_std_factor = 0.5;
- Scalar gps_std = 1;
+ double odom_std_factor = 0.5;
+ double gps_std = 1;
std::default_random_engine generator(1);
- std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise
- std::normal_distribution<Scalar> distribution_gps(0.0, gps_std); //GPS noise
+ std::normal_distribution<double> distribution_odom(0.0, odom_std_factor); //odometry noise
+ std::normal_distribution<double> distribution_gps(0.0, gps_std); //GPS noise
//variables
- Eigen::Vector2s odom_data;
- Eigen::Vector2s gps_fix_reading;
- Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
- Eigen::Vector3s ground_truth_pose; //last true pose
- Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
- Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
+ Eigen::Vector2d odom_data;
+ Eigen::Vector2d gps_fix_reading;
+ Eigen::VectorXd ground_truth(n_execution * 3); //all true poses
+ Eigen::Vector3d ground_truth_pose; //last true pose
+ Eigen::VectorXd odom_trajectory(n_execution * 3); //open loop trajectory
+ Eigen::VectorXd mean_times = Eigen::VectorXd::Zero(7);
clock_t t1, t2;
- Scalar dt = 0.05;
+ double dt = 0.05;
TimeStamp ts(0);
// Wolf Tree 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
@@ -236,7 +236,7 @@ int main(int argc, char** argv)
problem.addSensor(laser_2_sensor);
problem.setProcessorMotion(odom_processor);
- CaptureMotion* odom_capture = new CaptureMotion(ts,odom_sensor, odom_data, Eigen::Matrix2s::Identity() * odom_std_factor * odom_std_factor, nullptr);
+ CaptureMotion* odom_capture = new CaptureMotion(ts,odom_sensor, odom_data, Eigen::Matrix2d::Identity() * odom_std_factor * odom_std_factor, nullptr);
// Simulated robot
FaramoticsRobot robot(argc, argv, laser_1_pose, laser_2_pose);
@@ -250,7 +250,7 @@ int main(int argc, char** argv)
FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts);
// Prior covariance
- CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1);
+ CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3d::Identity() * 0.1);
origin_frame->addCapture(initial_covariance);
initial_covariance->process();
@@ -313,7 +313,7 @@ int main(int argc, char** argv)
gps_fix_reading(0) += distribution_gps(generator);
gps_fix_reading(1) += distribution_gps(generator);
// process data
- //(new CaptureGPSFix(ts, &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
+ //(new CaptureGPSFix(ts, &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXd::Identity(3,3)));
}
mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
@@ -366,7 +366,7 @@ int main(int argc, char** argv)
// Print Final result in a file -------------------------
// Vehicle poses
int i = 0;
- Eigen::VectorXs state_poses = Eigen::VectorXs::Zero(n_execution * 3);
+ Eigen::VectorXd state_poses = Eigen::VectorXd::Zero(n_execution * 3);
for (auto frame : *(problem.getTrajectory()->getFrameList()))
{
state_poses.segment(i, 3) << frame->getP()->getVector(), frame->getO()->getVector();
@@ -375,7 +375,7 @@ int main(int argc, char** argv)
// Landmarks
i = 0;
- Eigen::VectorXs landmarks = Eigen::VectorXs::Zero(problem.getMap()->getLandmarkList()->size() * 2);
+ Eigen::VectorXd landmarks = Eigen::VectorXd::Zero(problem.getMap()->getLandmarkList()->size() * 2);
for (auto landmark : *(problem.getMap()->getLandmarkList()))
{
landmarks.segment(i, 2) = landmark->getP()->getVector();
diff --git a/demos/demo_ceres_2_lasers_polylines.cpp b/demos/demo_ceres_2_lasers_polylines.cpp
index d79bbfaf88853ee8ccec48f5c172ecce2b83e940..3bc78c0a4ab75fe55a31a8fbaf1fbf388ba201e0 100644
--- a/demos/demo_ceres_2_lasers_polylines.cpp
+++ b/demos/demo_ceres_2_lasers_polylines.cpp
@@ -48,10 +48,10 @@ class FaramoticsRobot
string modelFileName;
CrangeScan2D* myScanner;
CdynamicSceneRender* myRender;
- Eigen::Vector3s ground_truth_pose_;
- Eigen::Vector4s laser_1_pose_, laser_2_pose_;
+ Eigen::Vector3d ground_truth_pose_;
+ Eigen::Vector4d laser_1_pose_, laser_2_pose_;
- FaramoticsRobot(int argc, char** argv, const Eigen::Vector4s& _laser_1_pose, const Eigen::Vector4s& _laser_2_pose) :
+ FaramoticsRobot(int argc, char** argv, const Eigen::Vector4d& _laser_1_pose, const Eigen::Vector4d& _laser_2_pose) :
modelFileName("/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"),
laser_1_pose_(_laser_1_pose),
laser_2_pose_(_laser_2_pose)
@@ -72,7 +72,7 @@ class FaramoticsRobot
}
//function travel around
- Eigen::Vector3s motionCampus(unsigned int ii, double& displacement_, double& rotation_)
+ Eigen::Vector3d motionCampus(unsigned int ii, double& displacement_, double& rotation_)
{
if (ii <= 120){ displacement_ = 0.1; rotation_ = 0; }
else if (ii <= 170) { displacement_ = 0.2; rotation_ = 1.8 * M_PI / 180; }
@@ -126,7 +126,7 @@ class FaramoticsRobot
}
}
- void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3s& estimated_pose)
+ void render(const FeatureBasePtrList& feature_list, int laser, const LandmarkBasePtrList& landmark_list, const Eigen::Vector3d& estimated_pose)
{
// detected corners
std::cout << " drawCorners: " << feature_list.size() << std::endl;
@@ -146,7 +146,7 @@ class FaramoticsRobot
landmark_vector.reserve(3*landmark_list.size());
for (auto landmark : landmark_list)
{
- Scalar* position_ptr = landmark->getP()->get();
+ double* position_ptr = landmark->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -195,27 +195,27 @@ int main(int argc, char** argv)
// INITIALIZATION ============================================================================================
//init random generators
- Scalar odom_std_factor = 0.5;
- Scalar gps_std = 1;
+ double odom_std_factor = 0.5;
+ double gps_std = 1;
std::default_random_engine generator(1);
- std::normal_distribution<Scalar> distribution_odom(0.0, odom_std_factor); //odometry noise
- std::normal_distribution<Scalar> distribution_gps(0.0, gps_std); //GPS noise
+ std::normal_distribution<double> distribution_odom(0.0, odom_std_factor); //odometry noise
+ std::normal_distribution<double> distribution_gps(0.0, gps_std); //GPS noise
//variables
- Eigen::Vector2s odom_data;
- Eigen::Vector2s gps_fix_reading;
- Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
- Eigen::Vector3s ground_truth_pose; //last true pose
- Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
- Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
+ Eigen::Vector2d odom_data;
+ Eigen::Vector2d gps_fix_reading;
+ Eigen::VectorXd ground_truth(n_execution * 3); //all true poses
+ Eigen::Vector3d ground_truth_pose; //last true pose
+ Eigen::VectorXd odom_trajectory(n_execution * 3); //open loop trajectory
+ Eigen::VectorXd mean_times = Eigen::VectorXd::Zero(7);
clock_t t1, t2;
- Scalar dt = 0.05;
+ double dt = 0.05;
TimeStamp ts(0);
// Wolf Tree 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
@@ -243,7 +243,7 @@ int main(int argc, char** argv)
problem.addSensor(laser_2_sensor);
problem.setProcessorMotion(odom_processor);
- CaptureMotion* odom_capture = new CaptureMotion(ts,odom_sensor, odom_data, Eigen::Matrix2s::Identity() * odom_std_factor * odom_std_factor, nullptr);
+ CaptureMotion* odom_capture = new CaptureMotion(ts,odom_sensor, odom_data, Eigen::Matrix2d::Identity() * odom_std_factor * odom_std_factor, nullptr);
// Simulated robot
FaramoticsRobot robot(argc, argv, laser_1_pose, laser_2_pose);
@@ -257,7 +257,7 @@ int main(int argc, char** argv)
FrameBasePtr origin_frame = problem.createFrame(KEY, ground_truth_pose, ts);
// Prior covariance
- CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1);
+ CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3d::Identity() * 0.1);
origin_frame->addCapture(initial_covariance);
initial_covariance->process();
@@ -320,7 +320,7 @@ int main(int argc, char** argv)
gps_fix_reading(0) += distribution_gps(generator);
gps_fix_reading(1) += distribution_gps(generator);
// process data
- //(new CaptureGPSFix(ts, &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
+ //(new CaptureGPSFix(ts, &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXd::Identity(3,3)));
}
mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
diff --git a/include/laser/factor/factor_point_2D.h b/include/laser/factor/factor_point_2D.h
index 365fcca6047a5dae77f7067570bcd88104f834fd..817891d9965496e61c8e6c2c5183356134370ff5 100644
--- a/include/laser/factor/factor_point_2D.h
+++ b/include/laser/factor/factor_point_2D.h
@@ -19,9 +19,9 @@ class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
unsigned int feature_point_id_;
int landmark_point_id_;
StateBlockPtr point_state_ptr_;
- Eigen::VectorXs measurement_; ///< the measurement vector
- Eigen::MatrixXs measurement_covariance_; ///< the measurement covariance matrix
- Eigen::MatrixXs measurement_sqrt_information_; ///< the squared root information matrix
+ Eigen::VectorXd measurement_; ///< the measurement vector
+ Eigen::MatrixXd measurement_covariance_; ///< the measurement covariance matrix
+ Eigen::MatrixXd measurement_sqrt_information_; ///< the squared root information matrix
public:
@@ -35,8 +35,8 @@ class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
{
//std::cout << "Constriant point: feature " << _ftr_ptr->id() << " landmark " << _lmk_ptr->id() << "(point " << _lmk_point_id << ")" << std::endl;
//std::cout << "landmark state block " << _lmk_ptr->getPointStateBlock(_lmk_point_id)->getVector().transpose() << std::endl;
- Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
- Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
+ Eigen::LLT<Eigen::MatrixXd> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
+ Eigen::MatrixXd measurement_sqrt_covariance = lltOfA.matrixU();
measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
}
@@ -86,21 +86,21 @@ class FactorPoint2D: public FactorAutodiff<FactorPoint2D, 2,2,1,2,1,2>
/** \brief Returns a reference to the feature measurement
**/
- virtual const Eigen::VectorXs& getMeasurement() const override
+ virtual const Eigen::VectorXd& getMeasurement() const override
{
return measurement_;
}
/** \brief Returns a reference to the feature measurement covariance
**/
- virtual const Eigen::MatrixXs& getMeasurementCovariance() const override
+ virtual const Eigen::MatrixXd& getMeasurementCovariance() const override
{
return measurement_covariance_;
}
/** \brief Returns a reference to the feature measurement square root information
**/
- virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const override
+ virtual const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const override
{
return measurement_sqrt_information_;
}
diff --git a/include/laser/factor/factor_point_to_line_2D.h b/include/laser/factor/factor_point_to_line_2D.h
index 30a3c6edff9b92f7e35ce6020a3fdb1f2c3d6170..fd7f25c73e9dcd7361f02a5d04fe1e7c0b7ec9a4 100644
--- a/include/laser/factor/factor_point_to_line_2D.h
+++ b/include/laser/factor/factor_point_to_line_2D.h
@@ -19,9 +19,9 @@ class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,
unsigned int feature_point_id_;
StateBlockPtr point_state_ptr_;
StateBlockPtr point_aux_state_ptr_;
- Eigen::VectorXs measurement_; ///< the measurement vector
- Eigen::MatrixXs measurement_covariance_; ///< the measurement covariance matrix
- Eigen::MatrixXs measurement_sqrt_information_; ///< the squared root information matrix
+ Eigen::VectorXd measurement_; ///< the measurement vector
+ Eigen::MatrixXd measurement_covariance_; ///< the measurement covariance matrix
+ Eigen::MatrixXd measurement_sqrt_information_; ///< the squared root information matrix
public:
@@ -37,8 +37,8 @@ class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,
//std::cout << "FactorPointToLine2D" << std::endl;
//std::cout << "Landmark " << _lmk_ptr->id() << " first " << _lmk_ptr->getFirstId() << ", last " << _lmk_ptr->getLastId() << " isValid(ctr points):" << (_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_) ? "YES" : "NO") << std::endl;
assert(_lmk_ptr->isValidId(landmark_point_id_) && _lmk_ptr->isValidId(landmark_point_aux_id_));
- Eigen::LLT<Eigen::MatrixXs> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
- Eigen::MatrixXs measurement_sqrt_covariance = lltOfA.matrixU();
+ Eigen::LLT<Eigen::MatrixXd> lltOfA(measurement_covariance_); // compute the Cholesky decomposition of A
+ Eigen::MatrixXd measurement_sqrt_covariance = lltOfA.matrixU();
measurement_sqrt_information_ = measurement_sqrt_covariance.inverse().transpose(); // retrieve factor U in the decomposition
}
@@ -79,21 +79,21 @@ class FactorPointToLine2D: public FactorAutodiff<FactorPointToLine2D, 1,2,1,2,1,
/** \brief Returns a reference to the feature measurement
**/
- virtual const Eigen::VectorXs& getMeasurement() const override
+ virtual const Eigen::VectorXd& getMeasurement() const override
{
return measurement_;
}
/** \brief Returns a reference to the feature measurement covariance
**/
- virtual const Eigen::MatrixXs& getMeasurementCovariance() const override
+ virtual const Eigen::MatrixXd& getMeasurementCovariance() const override
{
return measurement_covariance_;
}
/** \brief Returns a reference to the feature measurement square root information
**/
- virtual const Eigen::MatrixXs& getMeasurementSquareRootInformationUpper() const override
+ virtual const Eigen::MatrixXd& getMeasurementSquareRootInformationUpper() const override
{
return measurement_sqrt_information_;
}
diff --git a/include/laser/feature/feature_match_polyline_2D.h b/include/laser/feature/feature_match_polyline_2D.h
index e41da2f96e07930d9e5916402854554b2c16e7c8..820edf0d7d1ceea03c94a0ccd98ce8cbcb7a0851 100644
--- a/include/laser/feature/feature_match_polyline_2D.h
+++ b/include/laser/feature/feature_match_polyline_2D.h
@@ -23,7 +23,7 @@ struct FeatureMatchPolyline2D : public FeatureMatch
int feature_last_to_id_;
int feature_incoming_from_id_;
int feature_incoming_to_id_;
- Eigen::Matrix3s T_incoming_last_;
+ Eigen::Matrix3d T_incoming_last_;
FeaturePolyline2DPtr pl_incoming_;
FeaturePolyline2DPtr pl_last_;
@@ -33,7 +33,7 @@ struct FeatureMatchPolyline2D : public FeatureMatch
*
* Returns an array containing the squared distances of each point of the match
*/
- Eigen::Array<Scalar,Eigen::Dynamic,1> computeSqDistArray() const;
+ Eigen::Array<double,Eigen::Dynamic,1> computeSqDistArray() const;
void print() const;
diff --git a/include/laser/feature/feature_polyline_2D.h b/include/laser/feature/feature_polyline_2D.h
index 7691d1aec8300f62514c2e6f41b68603677c1bba..5ff5714733386341e1e99f7971a025df6667904d 100644
--- a/include/laser/feature/feature_polyline_2D.h
+++ b/include/laser/feature/feature_polyline_2D.h
@@ -19,23 +19,28 @@ WOLF_LIST_TYPEDEFS(FeaturePolyline2D);
class FeaturePolyline2D : public FeatureBase
{
protected:
- Eigen::MatrixXs points_; // homogeneous 2D points (each column is a point)
- Eigen::MatrixXs points_cov_;
+ Eigen::MatrixXd points_; // homogeneous 2D points (each column is a point)
+ Eigen::MatrixXd points_cov_;
bool first_defined_;
bool last_defined_;
public:
- FeaturePolyline2D(const Eigen::MatrixXs& _points, const Eigen::MatrixXs& _points_cov, const bool& _first_defined, const bool& _last_defined);
+ FeaturePolyline2D(const Eigen::MatrixXd& _points, const Eigen::MatrixXd& _points_cov, const bool& _first_defined, const bool& _last_defined);
virtual ~FeaturePolyline2D();
- const Eigen::MatrixXs& getPoints() const;
- const Eigen::MatrixXs& getPointsCov() const;
+ const Eigen::MatrixXd& getPoints() const;
+ const Eigen::MatrixXd& getPointsCov() const;
bool isFirstDefined() const;
bool isLastDefined() const;
int getNPoints() const;
};
-inline FeaturePolyline2D::FeaturePolyline2D(const Eigen::MatrixXs& _points, const Eigen::MatrixXs& _points_cov, const bool& _first_defined, const bool& _last_defined) :
- FeatureBase("FeaturePolyline2D", Eigen::Vector1s::Zero(), Eigen::Vector1s::Ones()),
+//<<<<<<< HEAD
+inline FeaturePolyline2D::FeaturePolyline2D(const Eigen::MatrixXd& _points, const Eigen::MatrixXd& _points_cov, const bool& _first_defined, const bool& _last_defined) :
+ FeatureBase("FeaturePolyline2D", Eigen::Vector1d::Zero(), Eigen::Vector1d::Ones()),
+//=======
+//inline FeaturePolyline2D::FeaturePolyline2D(const Eigen::MatrixXd& _points, const Eigen::MatrixXd& _points_cov, const bool& _first_defined, const bool& _last_defined) :
+// FeatureBase("POLYLINE 2D", Eigen::Vector1d::Zero(), Eigen::Vector1d::Ones()),
+//>>>>>>> 8-replace-scalar-to-double
points_(_points),
points_cov_(_points_cov),
first_defined_(_first_defined),
@@ -49,12 +54,12 @@ inline FeaturePolyline2D::~FeaturePolyline2D()
//
}
-inline const Eigen::MatrixXs& FeaturePolyline2D::getPoints() const
+inline const Eigen::MatrixXd& FeaturePolyline2D::getPoints() const
{
return points_;
}
-inline const Eigen::MatrixXs& FeaturePolyline2D::getPointsCov() const
+inline const Eigen::MatrixXd& FeaturePolyline2D::getPointsCov() const
{
return points_cov_;
}
diff --git a/include/laser/landmark/landmark_match_polyline_2D.h b/include/laser/landmark/landmark_match_polyline_2D.h
index d6e80316b25fce24c12b4b57ef1ff3ee2812fd46..fd3ecdad382f8808412aa377561c7e406ca430c0 100644
--- a/include/laser/landmark/landmark_match_polyline_2D.h
+++ b/include/laser/landmark/landmark_match_polyline_2D.h
@@ -27,7 +27,7 @@ struct LandmarkMatchPolyline2D : public LandmarkMatch
int landmark_to_id_;
int feature_to_id_;
int landmark_version_;
- Eigen::Matrix3s T_feature_landmark_;
+ Eigen::Matrix3d T_feature_landmark_;
FeaturePolyline2DPtr pl_feature_;
LandmarkPolyline2DPtr pl_landmark_;
@@ -50,7 +50,7 @@ struct LandmarkMatchPolyline2D : public LandmarkMatch
*
* Returns an array containing the squared distances of each point of the match
*/
- Eigen::Array<Scalar,Eigen::Dynamic,1> computeSqDistArray() const;
+ Eigen::Array<double,Eigen::Dynamic,1> computeSqDistArray() const;
void print() const;
diff --git a/include/laser/landmark/landmark_polyline_2D.h b/include/laser/landmark/landmark_polyline_2D.h
index 8d8ce4b3e10c456cec2a8e02a6c74fab18c97570..3aa160cf34707d82002a45980cc980bae2b0afe9 100644
--- a/include/laser/landmark/landmark_polyline_2D.h
+++ b/include/laser/landmark/landmark_polyline_2D.h
@@ -31,9 +31,9 @@ typedef enum
struct PolylineRectangularClass
{
PolylineClassType type; // name of the type
- Scalar L; // long side length
- Scalar W; // short side length
- Scalar D; // diagonal length
+ double L; // long side length
+ double W; // short side length
+ double D; // diagonal length
PolylineRectangularClass() :
type(UNCLASSIFIED),
@@ -43,7 +43,7 @@ struct PolylineRectangularClass
{
};
- PolylineRectangularClass(const PolylineClassType& _type, const Scalar& _L, const Scalar& _W) :
+ PolylineRectangularClass(const PolylineClassType& _type, const double& _L, const double& _W) :
type(_type),
L(_L),
W(_W),
@@ -85,8 +85,8 @@ class LandmarkPolyline2D : public LandmarkBase
StateBlockPtr& lastStateBlock();
public:
- LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id = 0);
- LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class);
+ LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id = 0);
+ LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class);
virtual ~LandmarkPolyline2D();
/** \brief Gets a const reference to the point state block pointer vector
@@ -117,8 +117,8 @@ class LandmarkPolyline2D : public LandmarkBase
/** \brief Sets the first/last extreme point
**/
- void setFirst(const Eigen::VectorXs& _point, bool _defined);
- void setLast(const Eigen::VectorXs& _point, bool _defined);
+ void setFirst(const Eigen::VectorXd& _point, bool _defined);
+ void setLast(const Eigen::VectorXd& _point, bool _defined);
int getNPoints() const;
int getNDefinedPoints() const;
@@ -131,11 +131,11 @@ class LandmarkPolyline2D : public LandmarkBase
int getPrevValidId(const int& i) const;
int getVersion() const;
- const Eigen::VectorXs getPointVector(int _i) const;
+ const Eigen::VectorXd getPointVector(int _i) const;
- Eigen::MatrixXs computePointsGlobal() const;
- Eigen::MatrixXs computePointsGlobal(const int& _from_id, const int& _to_id) const;
- Eigen::MatrixXs computePointsCovGlobal() const;
+ Eigen::MatrixXd computePointsGlobal() const;
+ Eigen::MatrixXd computePointsGlobal(const int& _from_id, const int& _to_id) const;
+ Eigen::MatrixXd computePointsCovGlobal() const;
StateBlockPtr getPointStateBlock(int _i);
@@ -148,7 +148,7 @@ class LandmarkPolyline2D : public LandmarkBase
* \param _extreme: if its extreme or not
* \param _back: if it have to be added in the back (true) or in the front (false)
**/
- void addPoint(const Eigen::VectorXs& _point, const bool& _defined, const bool& _back);
+ void addPoint(const Eigen::VectorXd& _point, const bool& _defined, const bool& _back);
/** \brief Adds new points to the landmark
* \param _points: a matrix containing points, some of them to be added
@@ -156,7 +156,7 @@ class LandmarkPolyline2D : public LandmarkBase
* \param _extreme: if last point to be added is extreme or not
* \param _back: if the points have to be added in the back (true) or in the front (false)
**/
- void addPoints(const Eigen::MatrixXs& _points, const unsigned int& _idx, const bool& _defined, const bool& _back);
+ void addPoints(const Eigen::MatrixXd& _points, const unsigned int& _idx, const bool& _defined, const bool& _back);
/** \brief define extreme point
**/
@@ -172,7 +172,7 @@ class LandmarkPolyline2D : public LandmarkBase
/** \brief Try to close the polyline (if it has overlapped points)
**/
- virtual bool tryClose(const Scalar& _dist_tol);
+ virtual bool tryClose(const double& _dist_tol);
/** \brief Set the polyline as closed
**/
@@ -188,7 +188,7 @@ class LandmarkPolyline2D : public LandmarkBase
/** \brief Try to classify polyline as a known object
**/
- bool tryClassify(const Scalar& _dist_tol, std::vector<PolylineRectangularClass> _classes);
+ bool tryClassify(const double& _dist_tol, std::vector<PolylineRectangularClass> _classes);
/** \brief Try to classify polyline as a known object
**/
@@ -210,8 +210,8 @@ class LandmarkPolyline2D : public LandmarkBase
YAML::Node saveToYaml() const;
static void tryMergeLandmarks(LandmarkPolyline2DPtrList& _lmk_list,
- const Scalar& max_sq_error,
- const Scalar& min_overlapping_dist,
+ const double& max_sq_error,
+ const double& min_overlapping_dist,
const int& min_N_overlapped,
const int& min_N_defined_overlapped);
};
diff --git a/include/laser/processor/polyline_2D_utils.h b/include/laser/processor/polyline_2D_utils.h
index 62be7c00d128074128eeec92445a83846ef32384..c3724c6f9f0c86145bdf85b26d1d4abdb0238c86 100644
--- a/include/laser/processor/polyline_2D_utils.h
+++ b/include/laser/processor/polyline_2D_utils.h
@@ -18,7 +18,7 @@
namespace wolf {
WOLF_STRUCT_PTR_TYPEDEFS(MatchPolyline2D);
-typedef std::map<Scalar,MatchPolyline2DPtr> MatchPolyline2DMap;
+typedef std::map<double,MatchPolyline2DPtr> MatchPolyline2DMap;
typedef std::list<MatchPolyline2DPtr> MatchPolyline2DList;
/** \brief Match between a two polylines (not specialized to landmark nor feature)
@@ -32,9 +32,9 @@ struct MatchPolyline2D
int to_A_id_;
int from_B_id_;
int to_B_id_;
- Scalar normalized_score_;
- Scalar overlap_dist_;
- Eigen::Matrix3s T_A_B_;
+ double normalized_score_;
+ double overlap_dist_;
+ Eigen::Matrix3d T_A_B_;
void print() const
{
@@ -54,43 +54,43 @@ struct MatchPolyline2D
}
};
-Eigen::Matrix3s pose2T2D(const Eigen::Vector2s& t, const Scalar& theta);
-Eigen::Matrix3s pose2T2D(const Eigen::Vector3s& pose);
-Eigen::Vector3s T2pose2D(const Eigen::Matrix3s& T);
+Eigen::Matrix3d pose2T2D(const Eigen::Vector2d& t, const double& theta);
+Eigen::Matrix3d pose2T2D(const Eigen::Vector3d& pose);
+Eigen::Vector3d T2pose2D(const Eigen::Matrix3d& T);
-Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_incoming, const Eigen::MatrixXs& _points_last, const bool& first_defined = true, const bool& last_defined=true);
+Eigen::Vector3d computeRigidTrans(const Eigen::MatrixXd& _points_incoming, const Eigen::MatrixXd& _points_last, const bool& first_defined = true, const bool& last_defined=true);
/** \brief Computes the squared distance from a point to a segment defined by two points
**/
-Scalar sqDistPoint2Segment(const Eigen::Vector2s& p, const Eigen::Vector2s& a, const Eigen::Vector2s& b);
+double sqDistPoint2Segment(const Eigen::Vector2d& p, const Eigen::Vector2d& a, const Eigen::Vector2d& b);
-Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
- const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined, bool strict=false);
+double sqDistPointToLine(const Eigen::Vector3d& _A, const Eigen::Vector3d& _A_aux,
+ const Eigen::Vector3d& _B, bool _A_defined, bool _B_defined, bool strict=false);
-MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
- const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
- const Scalar& max_sq_error,
- const Scalar& min_overlapping_dist,
+MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXd& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
+ const Eigen::MatrixXd& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
+ const double& max_sq_error,
+ const double& min_overlapping_dist,
const int& min_N_overlapped,
const int& min_N_defined_overlapped,
bool both_exceeding_A_allowed_ = true,
bool both_exceeding_B_allowed_ = true);
-Eigen::Array<Scalar,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXs& _points_A, const int& from_A, const int& to_A, bool from_A_defined, bool to_A_defined,
- const Eigen::MatrixXs& _points_B, const int& from_B, const int& to_B, bool from_B_defined, bool to_B_defined,
+Eigen::Array<double,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXd& _points_A, const int& from_A, const int& to_A, bool from_A_defined, bool to_A_defined,
+ const Eigen::MatrixXd& _points_B, const int& from_B, const int& to_B, bool from_B_defined, bool to_B_defined,
const int& last_A, const int& N_overlapped );
-MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
- const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
- const Scalar& max_sq_error );
+MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXd& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
+ const Eigen::MatrixXd& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
+ const double& max_sq_error );
-bool isProjectedOverSegment(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B);
+bool isProjectedOverSegment(const Eigen::Vector3d& _A, const Eigen::Vector3d& _A_aux, const Eigen::Vector3d& _B);
-Eigen::VectorXs computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
- const Eigen::Matrix2s& _feature_cov,
- const Eigen::Vector2s& _expected_feature,
- const Eigen::Matrix2s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu);
+Eigen::VectorXd computeSquaredMahalanobisDistances(const Eigen::Vector2d& _feature,
+ const Eigen::Matrix2d& _feature_cov,
+ const Eigen::Vector2d& _expected_feature,
+ const Eigen::Matrix2d& _expected_feature_cov,
+ const Eigen::MatrixXd& _mu);
} // namespace wolf
diff --git a/include/laser/processor/processor_tracker_feature_polyline_2D.h b/include/laser/processor/processor_tracker_feature_polyline_2D.h
index 289f82becb18b543d67bc139bb2de070a1547fb1..70d7df5f0d7632c0fb46a38c8737f564efcf9942 100644
--- a/include/laser/processor/processor_tracker_feature_polyline_2D.h
+++ b/include/laser/processor/processor_tracker_feature_polyline_2D.h
@@ -33,8 +33,8 @@ WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline2D);
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline2D);
typedef std::list<FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DPtrList;
typedef std::list<LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DPtrList;
-typedef std::map<Scalar,FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DScalarMap;
-typedef std::map<Scalar,LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DScalarMap;
+typedef std::map<double,FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DScalarMap;
+typedef std::map<double,LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DScalarMap;
struct ProcessorParamsTrackerFeaturePolyline2D : public ProcessorParamsTrackerFeature
{
@@ -51,25 +51,25 @@ struct ProcessorParamsTrackerFeaturePolyline2D : public ProcessorParamsTrackerFe
}
laserscanutils::LineFinderIterativeParams line_finder_params;
- Scalar match_feature_position_sq_norm_max;
- Scalar match_feature_orientation_sq_norm_max;
+ double match_feature_position_sq_norm_max;
+ double match_feature_orientation_sq_norm_max;
int match_feature_overlap_n_min;
int match_feature_overlap_n_defined_min;
- Scalar match_feature_overlap_dist_min;
+ double match_feature_overlap_dist_min;
- Scalar match_landmark_position_sq_norm_max;
- Scalar match_landmark_orientation_sq_norm_max;
+ double match_landmark_position_sq_norm_max;
+ double match_landmark_orientation_sq_norm_max;
int match_landmark_overlap_n_min;
int match_landmark_overlap_n_defined_min;
- Scalar match_landmark_overlap_dist_min;
+ double match_landmark_overlap_dist_min;
bool use_probabilistic_match;
unsigned int new_features_th;
- Scalar loop_time_th;
+ double loop_time_th;
std::vector<PolylineRectangularClass> polyline_classes;
- Scalar class_position_error_th;
+ double class_position_error_th;
};
class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
@@ -83,14 +83,14 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
LandmarkPolyline2DPtrList modified_lmks_;
std::list<LandmarkPolyline2DPtrList> merge_candidates_list_;
- Eigen::Matrix2s R_sensor_world_last_, R_world_sensor_last_;
- Eigen::Vector2s t_sensor_world_last_, t_world_sensor_last_;
- Eigen::Matrix2s R_sensor_world_incoming_, R_world_sensor_incoming_;
- Eigen::Vector2s t_sensor_world_incoming_, t_world_sensor_incoming_;
- Eigen::Matrix2s R_robot_sensor_;
- Eigen::Vector2s t_robot_sensor_;
- Eigen::Matrix2s R_last_incoming_, R_incoming_last_;
- Eigen::Vector2s t_last_incoming_, t_incoming_last_;
+ Eigen::Matrix2d R_sensor_world_last_, R_world_sensor_last_;
+ Eigen::Vector2d t_sensor_world_last_, t_world_sensor_last_;
+ Eigen::Matrix2d R_sensor_world_incoming_, R_world_sensor_incoming_;
+ Eigen::Vector2d t_sensor_world_incoming_, t_world_sensor_incoming_;
+ Eigen::Matrix2d R_robot_sensor_;
+ Eigen::Vector2d t_robot_sensor_;
+ Eigen::Matrix2d R_last_incoming_, R_incoming_last_;
+ Eigen::Vector2d t_last_incoming_, t_incoming_last_;
bool extrinsics_transformation_computed_;
public:
@@ -230,12 +230,12 @@ class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature
void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
const FeaturePolyline2DPtr& _ftr_last,
const FeaturePolyline2DPtr& _ftr_incoming,
- const Eigen::Matrix3s& _T_last_incoming_prior) const;
+ const Eigen::Matrix3d& _T_last_incoming_prior) const;
void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
const LandmarkPolyline2DPtr& _lmk_ptr,
const FeaturePolyline2DPtr& _feat_ptr,
- const Eigen::Matrix3s& _T_feature_landmark_prior) const;
+ const Eigen::Matrix3d& _T_feature_landmark_prior) const;
bool updateLandmarkMatch(LandmarkMatchPolyline2DPtr&) const;
diff --git a/include/laser/sensor/sensor_laser_2D.h b/include/laser/sensor/sensor_laser_2D.h
index fd1809d2a8ead43cbd54806caf6c3db3fa2a7fa7..4c77ad4469f8b2c1c4b43c65c6cda4eb91951019 100644
--- a/include/laser/sensor/sensor_laser_2D.h
+++ b/include/laser/sensor/sensor_laser_2D.h
@@ -83,7 +83,7 @@ class SensorLaser2D : public SensorBase
SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const laserscanutils::LaserScanParams& _params);
SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const IntrinsicsLaser2D& _params);
SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, IntrinsicsLaser2DPtr _params);
- SensorLaser2D(const Eigen::VectorXs& _extrinsics, IntrinsicsLaser2DPtr _params);
+ SensorLaser2D(const Eigen::VectorXd& _extrinsics, IntrinsicsLaser2DPtr _params);
WOLF_SENSOR_CREATE(SensorLaser2D, IntrinsicsLaser2D, 3);
virtual ~SensorLaser2D();
diff --git a/include/laser/state_block/local_parametrization_polyline_extreme.h b/include/laser/state_block/local_parametrization_polyline_extreme.h
index ce025e7404b0cde7f67a3521255b3f6597302b36..f57dd3a4a50d70843cf9e2307fd34c656e0ee8a1 100644
--- a/include/laser/state_block/local_parametrization_polyline_extreme.h
+++ b/include/laser/state_block/local_parametrization_polyline_extreme.h
@@ -24,13 +24,13 @@ class LocalParametrizationPolylineExtreme : public LocalParametrizationBase
LocalParametrizationPolylineExtreme(StateBlockPtr _reference_point);
virtual ~LocalParametrizationPolylineExtreme();
- virtual bool plus(Eigen::Map<const Eigen::VectorXs>& _point,
- Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _point_plus_delta_theta) const;
- virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXs>& _point, Eigen::Map<Eigen::MatrixXs>& _jacobian) const;
- virtual bool minus(Eigen::Map<const Eigen::VectorXs>& _point1,
- Eigen::Map<const Eigen::VectorXs>& _point2,
- Eigen::Map<Eigen::VectorXs>& _delta_theta);
+ virtual bool plus(Eigen::Map<const Eigen::VectorXd>& _point,
+ Eigen::Map<const Eigen::VectorXd>& _delta_theta,
+ Eigen::Map<Eigen::VectorXd>& _point_plus_delta_theta) const;
+ virtual bool computeJacobian(Eigen::Map<const Eigen::VectorXd>& _point, Eigen::Map<Eigen::MatrixXd>& _jacobian) const;
+ virtual bool minus(Eigen::Map<const Eigen::VectorXd>& _point1,
+ Eigen::Map<const Eigen::VectorXd>& _point2,
+ Eigen::Map<Eigen::VectorXd>& _delta_theta);
};
} // namespace wolf
diff --git a/src/feature/feature_match_polyline_2D.cpp b/src/feature/feature_match_polyline_2D.cpp
index a5383d16173ac8c2bda04a25f23bbd0d563c9c40..78cfadc7f8e2385d5e05463e12f2c98f843de52c 100644
--- a/src/feature/feature_match_polyline_2D.cpp
+++ b/src/feature/feature_match_polyline_2D.cpp
@@ -110,7 +110,7 @@ bool FeatureMatchPolyline2D::check(bool check_partial_match) const
return true;
}
-Eigen::Array<Scalar,Eigen::Dynamic,1> FeatureMatchPolyline2D::computeSqDistArray() const
+Eigen::Array<double,Eigen::Dynamic,1> FeatureMatchPolyline2D::computeSqDistArray() const
{
assert(pl_incoming_ != nullptr);
assert(pl_last_ != nullptr);
diff --git a/src/landmark/landmark_match_polyline_2D.cpp b/src/landmark/landmark_match_polyline_2D.cpp
index add363ba36bf6a26c5191bfff801d867ff565ecb..edb85dd98d617a6a7298e8a8046831cbd430f840 100644
--- a/src/landmark/landmark_match_polyline_2D.cpp
+++ b/src/landmark/landmark_match_polyline_2D.cpp
@@ -170,7 +170,7 @@ bool LandmarkMatchPolyline2D::check(bool check_partial_match) const
return true;
}
-Eigen::Array<Scalar,Eigen::Dynamic,1> LandmarkMatchPolyline2D::computeSqDistArray() const
+Eigen::Array<double,Eigen::Dynamic,1> LandmarkMatchPolyline2D::computeSqDistArray() const
{
assert(pl_feature_ != nullptr);
assert(pl_landmark_ != nullptr);
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index 7b1edfbed54edd5b1b708c0517560b0417540c45..520f7bf0591a5d0974ef359c569ef687267dc940 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -18,13 +18,18 @@ namespace wolf
{
-LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id) :
+LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id) :
LandmarkPolyline2D(_p_ptr, _o_ptr, _points, _first_defined, _last_defined, _first_id, PolylineRectangularClass())
{
}
-LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXs& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class) :
+//<<<<<<< HEAD
+LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class) :
LandmarkBase("LandmarkPolyline2D", _p_ptr, _o_ptr), first_defined_(_first_defined), last_defined_(_last_defined), closed_(false), classification_(_class), version_(0), merged_in_lmk_(nullptr)
+//=======
+//LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const Eigen::MatrixXd& _points, const bool _first_defined, const bool _last_defined, unsigned int _first_id, PolylineRectangularClass _class) :
+// LandmarkBase("POLYLINE 2D", _p_ptr, _o_ptr), first_defined_(_first_defined), last_defined_(_last_defined), closed_(false), classification_(_class), version_(0), merged_in_lmk_(nullptr)
+//>>>>>>> 8-replace-scalar-to-double
{
//std::cout << "LandmarkPolyline2D::LandmarkPolyline2D" << std::endl;
assert(_points.cols() >= 2 && "2 points at least needed.");
@@ -45,7 +50,7 @@ LandmarkPolyline2D::~LandmarkPolyline2D()
removeStateBlocks();
}
-void LandmarkPolyline2D::setFirst(const Eigen::VectorXs& _point, bool _defined)
+void LandmarkPolyline2D::setFirst(const Eigen::VectorXd& _point, bool _defined)
{
//std::cout << "LandmarkPolyline2D::setFirst. Defined " << _defined << std::endl;
assert(_point.size() >= 2 && "LandmarkPolyline2D::setFirstExtreme: bad point size");
@@ -55,7 +60,7 @@ void LandmarkPolyline2D::setFirst(const Eigen::VectorXs& _point, bool _defined)
defineFirst();
}
-void LandmarkPolyline2D::setLast(const Eigen::VectorXs& _point, bool _defined)
+void LandmarkPolyline2D::setLast(const Eigen::VectorXd& _point, bool _defined)
{
//std::cout << "LandmarkPolyline2D::setLast. Defined " << _defined << std::endl;
assert(_point.size() >= 2 && "LandmarkPolyline2D::setLastExtreme: bad point size");
@@ -65,7 +70,7 @@ void LandmarkPolyline2D::setLast(const Eigen::VectorXs& _point, bool _defined)
defineLast();
}
-const Eigen::VectorXs LandmarkPolyline2D::getPointVector(int _i) const
+const Eigen::VectorXd LandmarkPolyline2D::getPointVector(int _i) const
{
assert(point_state_ptr_map_.find(_i) != point_state_ptr_map_.end());
return point_state_ptr_map_.at(_i)->getState();
@@ -77,11 +82,11 @@ StateBlockPtr LandmarkPolyline2D::getPointStateBlock(int _i)
return point_state_ptr_map_.at(_i);
}
-Eigen::MatrixXs LandmarkPolyline2D::computePointsGlobal() const
+Eigen::MatrixXd LandmarkPolyline2D::computePointsGlobal() const
{
- Eigen::MatrixXs points_global = Eigen::MatrixXs::Ones(3,getNPoints());
- Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(getO()->getState()(0)).matrix();
- Eigen::Vector2s t_world_points = getP()->getState();
+ Eigen::MatrixXd points_global = Eigen::MatrixXd::Ones(3,getNPoints());
+ Eigen::Matrix2d R_world_points = Eigen::Rotation2Dd(getO()->getState()(0)).matrix();
+ Eigen::Vector2d t_world_points = getP()->getState();
for (int i = 0, valid_id = getFirstId(); i < getNPoints(); i++, valid_id = getNextValidId(valid_id))
{
@@ -98,13 +103,13 @@ Eigen::MatrixXs LandmarkPolyline2D::computePointsGlobal() const
return points_global;
}
-Eigen::MatrixXs LandmarkPolyline2D::computePointsGlobal(const int& _from_id, const int& _to_id) const
+Eigen::MatrixXd LandmarkPolyline2D::computePointsGlobal(const int& _from_id, const int& _to_id) const
{
assert((_from_id <= _to_id || closed_) && "wrong input");
int N_points = (_from_id <= _to_id ? id2idx(_to_id) - id2idx(_from_id) + 1 : getNPoints() - (id2idx(_from_id) - id2idx(_to_id)) + 1);
- Eigen::MatrixXs points_global = Eigen::MatrixXs::Ones(3,N_points);
+ Eigen::MatrixXd points_global = Eigen::MatrixXd::Ones(3,N_points);
int i = 0, valid_id = _from_id;
@@ -122,7 +127,7 @@ Eigen::MatrixXs LandmarkPolyline2D::computePointsGlobal(const int& _from_id, con
if (classification_.type != UNCLASSIFIED) // points are in landmark PO reference only when clasified
{
- Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(getO()->getState()(0)).matrix();
+ Eigen::Matrix2d R_world_points = Eigen::Rotation2Dd(getO()->getState()(0)).matrix();
points_global.topRows(2) = (R_world_points * points_global.topRows(2)).colwise() + getP()->getState();
}
@@ -131,14 +136,14 @@ Eigen::MatrixXs LandmarkPolyline2D::computePointsGlobal(const int& _from_id, con
-Eigen::MatrixXs LandmarkPolyline2D::computePointsCovGlobal() const
+Eigen::MatrixXd LandmarkPolyline2D::computePointsCovGlobal() const
{
- Eigen::MatrixXs points_cov_global = Eigen::MatrixXs::Zero(2,2*getNPoints());
+ Eigen::MatrixXd points_cov_global = Eigen::MatrixXd::Zero(2,2*getNPoints());
for (int i = 0, valid_id = getFirstId(); i < getNPoints(); i++, valid_id = getNextValidId(valid_id))
{
// TODO
- points_cov_global.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2);
+ points_cov_global.middleCols(i*2, 2) = Eigen::MatrixXd::Identity(2,2);
if (valid_id == getLastId())
break;
@@ -147,7 +152,7 @@ Eigen::MatrixXs LandmarkPolyline2D::computePointsCovGlobal() const
return points_cov_global;
}
-void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _defined, const bool& _back)
+void LandmarkPolyline2D::addPoint(const Eigen::VectorXd& _point, const bool& _defined, const bool& _back)
{
assert(!closed_ && "adding point to a closed polyline!");
@@ -192,7 +197,7 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de
version_++;
}
-void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigned int& _idx, const bool& _defined,
+void LandmarkPolyline2D::addPoints(const Eigen::MatrixXd& _points, const unsigned int& _idx, const bool& _defined,
const bool& _back)
{
assert(!closed_ && "adding points to a closed polyline!");
@@ -271,7 +276,7 @@ void LandmarkPolyline2D::defineExtreme(const bool _back)
first_defined_ = true;
}
-bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
+bool LandmarkPolyline2D::tryClose(const double& _dist_tol)
{
//std::cout << "tryClose landmark " << id() << " with tolerance = " << _dist_tol << std::endl;
@@ -351,7 +356,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
// Check that the other overlapped points are close enough (0 already checked)
for (auto i = 0; i < overlapped_ids.size(); i++)
{
- Scalar sq_dist=1e6;
+ double sq_dist=1e6;
// 2 defined points: point-point
if ( (overlapped_ids[i].first != getFirstId() || first_defined_) &&
@@ -387,10 +392,10 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
WOLF_TRACE("No overlapping found with defined points, trying with not defined extremes");
// Check for both not defined extremes that the distance to the segment of first and last defined points is below the threshold
- Eigen::Vector2s first_not_def = getPointVector(getFirstId());
- Eigen::Vector2s last_not_def = getPointVector(getLastId());
- Eigen::Vector2s first_def = getPointVector(getNextValidId(getFirstId()));
- Eigen::Vector2s last_def = getPointVector(getPrevValidId(getLastId()));
+ Eigen::Vector2d first_not_def = getPointVector(getFirstId());
+ Eigen::Vector2d last_not_def = getPointVector(getLastId());
+ Eigen::Vector2d first_def = getPointVector(getNextValidId(getFirstId()));
+ Eigen::Vector2d last_def = getPointVector(getPrevValidId(getLastId()));
if(sqDistPoint2Segment(first_not_def, last_def, first_def) < _dist_tol*_dist_tol &&
sqDistPoint2Segment(last_not_def, last_def, first_def) < _dist_tol*_dist_tol)
@@ -403,8 +408,8 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
// Also that the projection of them in the segment should be overlapped
// It means that last_not_def projected onto the segment (first_def-last_def) is closer to first_def than the projection of first_not_def
- Scalar last_proj_2_first_def = (last_def-first_def).dot(last_not_def -first_def) / (last_def-first_def).norm();
- Scalar first_proj_2_first_def = (last_def-first_def).dot(first_not_def-first_def) / (last_def-first_def).norm();
+ double last_proj_2_first_def = (last_def-first_def).dot(last_not_def -first_def) / (last_def-first_def).norm();
+ double first_proj_2_first_def = (last_def-first_def).dot(first_not_def-first_def) / (last_def-first_def).norm();
if (last_proj_2_first_def < first_proj_2_first_def)
{
@@ -551,7 +556,7 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
version_++;
}
-bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<PolylineRectangularClass> _classes)
+bool LandmarkPolyline2D::tryClassify(const double& _dist_tol, std::vector<PolylineRectangularClass> _classes)
{
//std::cout << "tryClassify landmark " << id() << " with " << getNDefinedPoints() << " defined points of " << getNPoints() << " - tol = " << _dist_tol << std::endl;
@@ -584,9 +589,9 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
points_ids.pop_back();
// consider 3 first defined points A-0, B-1, C-2 (and D-3)
- Scalar dAB = (getPointVector(points_ids[0]) - getPointVector(points_ids[1])).norm();
- Scalar dBC = (getPointVector(points_ids[1]) - getPointVector(points_ids[2])).norm();
- Scalar dAC = (getPointVector(points_ids[0]) - getPointVector(points_ids[2])).norm();
+ double dAB = (getPointVector(points_ids[0]) - getPointVector(points_ids[1])).norm();
+ double dBC = (getPointVector(points_ids[1]) - getPointVector(points_ids[2])).norm();
+ double dAC = (getPointVector(points_ids[0]) - getPointVector(points_ids[2])).norm();
//std::cout << "dAB = " << dAB << std::endl;
//std::cout << "dBC = " << dBC << std::endl;
@@ -628,9 +633,9 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
{
//std::cout << "checking with 4th point... " << std::endl;
- Scalar dAD = (getPointVector(points_ids[0]) - getPointVector(points_ids[3])).norm();
- Scalar dBD = (getPointVector(points_ids[1]) - getPointVector(points_ids[3])).norm();
- Scalar dCD = (getPointVector(points_ids[2]) - getPointVector(points_ids[3])).norm();
+ double dAD = (getPointVector(points_ids[0]) - getPointVector(points_ids[3])).norm();
+ double dBD = (getPointVector(points_ids[1]) - getPointVector(points_ids[3])).norm();
+ double dCD = (getPointVector(points_ids[2]) - getPointVector(points_ids[3])).norm();
// necessary conditions (rectangular shape) not fulfilled
if (fabs(dAD-dBC) > _dist_tol ||
@@ -659,7 +664,7 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
// Add a point if not enough points
if (getNPoints() < 4)
{
- addPoint(Eigen::Vector2s::Zero(), true, true);
+ addPoint(Eigen::Vector2d::Zero(), true, true);
points_ids.push_back(getLastId());
//std::cout << "ADDING POINT\n";
//std::cout << "\tdefined points: " << getNDefinedPoints() << "- points: " << getNPoints() << std::endl;
@@ -718,8 +723,8 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
// Move origin to to the center
getP()->setState((getPointVector(points_ids[0]) + getPointVector(points_ids[2])) / 2.0);
- Eigen::Vector2s frame_x = (configuration ? getPointVector(points_ids[0])-getPointVector(points_ids[1]) : getPointVector(points_ids[2])-getPointVector(points_ids[1]));
- getO()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0))));
+ Eigen::Vector2d frame_x = (configuration ? getPointVector(points_ids[0])-getPointVector(points_ids[1]) : getPointVector(points_ids[2])-getPointVector(points_ids[1]));
+ getO()->setState(Eigen::Vector1d::Constant(atan2(frame_x(1),frame_x(0))));
// Unfix origin
getP()->unfix();
@@ -735,17 +740,17 @@ bool LandmarkPolyline2D::tryClassify(const Scalar& _dist_tol, std::vector<Polyli
// Set polyline points to its respective relative positions
if (configuration)
{
- getPointStateBlock(points_ids[0])->setState(Eigen::Vector2s( classification_.L / 2,-classification_.W / 2));
- getPointStateBlock(points_ids[1])->setState(Eigen::Vector2s(-classification_.L / 2,-classification_.W / 2));
- getPointStateBlock(points_ids[2])->setState(Eigen::Vector2s(-classification_.L / 2, classification_.W / 2));
- getPointStateBlock(points_ids[3])->setState(Eigen::Vector2s( classification_.L / 2, classification_.W / 2));
+ getPointStateBlock(points_ids[0])->setState(Eigen::Vector2d( classification_.L / 2,-classification_.W / 2));
+ getPointStateBlock(points_ids[1])->setState(Eigen::Vector2d(-classification_.L / 2,-classification_.W / 2));
+ getPointStateBlock(points_ids[2])->setState(Eigen::Vector2d(-classification_.L / 2, classification_.W / 2));
+ getPointStateBlock(points_ids[3])->setState(Eigen::Vector2d( classification_.L / 2, classification_.W / 2));
}
else
{
- getPointStateBlock(points_ids[0])->setState(Eigen::Vector2s(-classification_.L / 2,-classification_.W / 2));
- getPointStateBlock(points_ids[1])->setState(Eigen::Vector2s(-classification_.L / 2, classification_.W / 2));
- getPointStateBlock(points_ids[2])->setState(Eigen::Vector2s( classification_.L / 2, classification_.W / 2));
- getPointStateBlock(points_ids[3])->setState(Eigen::Vector2s( classification_.L / 2,-classification_.W / 2));
+ getPointStateBlock(points_ids[0])->setState(Eigen::Vector2d(-classification_.L / 2,-classification_.W / 2));
+ getPointStateBlock(points_ids[1])->setState(Eigen::Vector2d(-classification_.L / 2, classification_.W / 2));
+ getPointStateBlock(points_ids[2])->setState(Eigen::Vector2d( classification_.L / 2, classification_.W / 2));
+ getPointStateBlock(points_ids[3])->setState(Eigen::Vector2d( classification_.L / 2,-classification_.W / 2));
}
for (auto st_pair : point_state_ptr_map_)
@@ -933,22 +938,22 @@ LandmarkBasePtr LandmarkPolyline2D::create(const YAML::Node& _lmk_node)
{
// Parse YAML node with lmk info and data
unsigned int id = _lmk_node["id"].as<unsigned int>();
- Eigen::VectorXs pos = _lmk_node["position"].as<Eigen::VectorXs>();
+ Eigen::VectorXd pos = _lmk_node["position"].as<Eigen::VectorXd>();
bool pos_fixed = true;//_lmk_node["position fixed"].as<bool>();
- Eigen::VectorXs ori = _lmk_node["orientation"].as<Eigen::VectorXs>();
+ Eigen::VectorXd ori = _lmk_node["orientation"].as<Eigen::VectorXd>();
bool ori_fixed = true;//_lmk_node["orientation fixed"].as<bool>();
int first_id = _lmk_node["first_id"].as<int>();
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();
int classif_type = _lmk_node["classification_type"].as<int>();
- Scalar classif_L = _lmk_node["classification_L"].as<Scalar>();
- Scalar classif_W = _lmk_node["classification_W"].as<Scalar>();
+ double classif_L = _lmk_node["classification_L"].as<double>();
+ double classif_W = _lmk_node["classification_W"].as<double>();
// load points
- 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
LandmarkPolyline2DPtr lmk_ptr = std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(pos, pos_fixed),
@@ -989,8 +994,8 @@ YAML::Node LandmarkPolyline2D::saveToYaml() const
}
void LandmarkPolyline2D::tryMergeLandmarks(LandmarkPolyline2DPtrList& _lmk_list,
- const Scalar& max_sq_error,
- const Scalar& min_overlapping_dist,
+ const double& max_sq_error,
+ const double& min_overlapping_dist,
const int& min_N_overlapped,
const int& min_N_defined_overlapped)
{
@@ -1015,8 +1020,8 @@ void LandmarkPolyline2D::tryMergeLandmarks(LandmarkPolyline2DPtrList& _lmk_list,
//std::cout << "Candidates to be merged: " << lmk_1->id() << " " << lmk_2->id() << "\n";
// check best correspondence
- Eigen::MatrixXs points_1 = lmk_1->computePointsGlobal();
- Eigen::MatrixXs points_2 = lmk_2->computePointsGlobal();
+ Eigen::MatrixXd points_1 = lmk_1->computePointsGlobal();
+ Eigen::MatrixXd points_2 = lmk_2->computePointsGlobal();
MatchPolyline2DPtr match = computeBestSqDist(points_1, lmk_1->isFirstDefined(), lmk_1->isLastDefined(), lmk_1->isClosed(),
points_2, lmk_2->isFirstDefined(), lmk_2->isLastDefined(), lmk_2->isClosed(),
max_sq_error,
diff --git a/src/processor/polyline_2D_utils.cpp b/src/processor/polyline_2D_utils.cpp
index 8aad5acc286b6bc2e2b1123a8edebd51421f8cac..48d28acd55fcc9d2197aa58b4e915723e4adc034 100644
--- a/src/processor/polyline_2D_utils.cpp
+++ b/src/processor/polyline_2D_utils.cpp
@@ -2,36 +2,36 @@
namespace wolf {
-Eigen::Matrix3s pose2T2D(const Eigen::Vector2s& t, const Scalar& theta)
+Eigen::Matrix3d pose2T2D(const Eigen::Vector2d& t, const double& theta)
{
- Eigen::Matrix3s T(Eigen::Matrix3s::Identity());
+ Eigen::Matrix3d T(Eigen::Matrix3d::Identity());
- T.topLeftCorner(2,2) = Eigen::Rotation2D<Scalar>(theta).matrix();
+ T.topLeftCorner(2,2) = Eigen::Rotation2D<double>(theta).matrix();
T.topRightCorner(2,1) = t;
return T;
}
-Eigen::Matrix3s pose2T2D(const Eigen::Vector3s& pose)
+Eigen::Matrix3d pose2T2D(const Eigen::Vector3d& pose)
{
- Eigen::Matrix3s T(Eigen::Matrix3s::Identity());
+ Eigen::Matrix3d T(Eigen::Matrix3d::Identity());
- T.topLeftCorner(2,2) = Eigen::Rotation2D<Scalar>(pose(2)).matrix();
+ T.topLeftCorner(2,2) = Eigen::Rotation2D<double>(pose(2)).matrix();
T.topRightCorner(2,1) = pose.head(2);
return T;
}
-Eigen::Vector3s T2pose2D(const Eigen::Matrix3s& T)
+Eigen::Vector3d T2pose2D(const Eigen::Matrix3d& T)
{
- Eigen::Vector3s pose;
+ Eigen::Vector3d pose;
pose.head(2) = T.topRightCorner(2,1);
pose(2) = atan2(T(1,0),T(0,0));
return pose;
}
-Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen::MatrixXs& _points_B, const bool& first_defined, const bool& last_defined)
+Eigen::Vector3d computeRigidTrans(const Eigen::MatrixXd& _points_A, const Eigen::MatrixXd& _points_B, const bool& first_defined, const bool& last_defined)
{
assert(_points_A.cols() == _points_B.cols());
assert(_points_A.rows() >= 2);
@@ -39,17 +39,17 @@ Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen:
int N_defined_points = _points_A.cols() - (first_defined ? 0 : 1) - (last_defined ? 0 : 1);
bool all_defined = first_defined && last_defined;
- Eigen::Vector3s t_A_B;
+ Eigen::Vector3d t_A_B;
assert(N_defined_points > 0 && _points_A.cols() > 1);
// Rotation
- Eigen::VectorXs angles_A_B(Eigen::VectorXs::Zero(_points_A.cols()));
- Eigen::Vector2s points_A_mean((all_defined ? _points_A.row(0).mean() : _points_A.row(0).segment(first_defined ? 0 : 1, N_defined_points).mean()),
+ Eigen::VectorXd angles_A_B(Eigen::VectorXd::Zero(_points_A.cols()));
+ Eigen::Vector2d points_A_mean((all_defined ? _points_A.row(0).mean() : _points_A.row(0).segment(first_defined ? 0 : 1, N_defined_points).mean()),
(all_defined ? _points_A.row(1).mean() : _points_A.row(1).segment(first_defined ? 0 : 1, N_defined_points).mean()));
- Eigen::Vector2s points_B_mean((all_defined ? _points_B.row(0).mean() : _points_B.row(0).segment(first_defined ? 0 : 1, N_defined_points).mean()),
+ Eigen::Vector2d points_B_mean((all_defined ? _points_B.row(0).mean() : _points_B.row(0).segment(first_defined ? 0 : 1, N_defined_points).mean()),
(all_defined ? _points_B.row(1).mean() : _points_B.row(1).segment(first_defined ? 0 : 1, N_defined_points).mean()));
- Eigen::MatrixXs points_A_centered = _points_A.topRows<2>().colwise()-points_A_mean;
- Eigen::MatrixXs points_B_centered = _points_B.topRows<2>().colwise()-points_B_mean;
+ Eigen::MatrixXd points_A_centered = _points_A.topRows<2>().colwise()-points_A_mean;
+ Eigen::MatrixXd points_B_centered = _points_B.topRows<2>().colwise()-points_B_mean;
for (auto i = 0; i < _points_A.cols(); i++ )
angles_A_B(i) = pi2pi(atan2(points_A_centered(1,i),points_A_centered(0,i)) - atan2(points_B_centered(1,i),points_B_centered(0,i)));
@@ -70,19 +70,19 @@ Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen:
t_A_B(2) = angles_A_B.segment(first_defined ? 0 : 1, N_defined_points).mean();
// Translation
- t_A_B.head<2>() = points_A_mean - Eigen::Rotation2D<Scalar>(t_A_B(2))*points_B_mean;
+ t_A_B.head<2>() = points_A_mean - Eigen::Rotation2D<double>(t_A_B(2))*points_B_mean;
return t_A_B;
}
/** \brief Computes the squared distance from a point to a segment defined by two points
**/
-Scalar sqDistPoint2Segment(const Eigen::Vector2s& p, const Eigen::Vector2s& a, const Eigen::Vector2s& b)
+double sqDistPoint2Segment(const Eigen::Vector2d& p, const Eigen::Vector2d& a, const Eigen::Vector2d& b)
{
- Scalar ap_sq = (p-a).squaredNorm();
- Scalar bp_sq = (p-b).squaredNorm();
- Scalar ab_sq = (b-a).squaredNorm();
- Scalar sqDist;
+ double ap_sq = (p-a).squaredNorm();
+ double bp_sq = (p-b).squaredNorm();
+ double ab_sq = (b-a).squaredNorm();
+ double sqDist;
// If the projection of p onto ab is inside the segment (acute triangle: all angles < 90º), return the sq distance point-line.
if (ap_sq <= bp_sq + ab_sq && bp_sq <= ap_sq + ab_sq) // acute triangle: applying pitagoras twice, angles ABP and PAB <= 90º
@@ -95,8 +95,8 @@ Scalar sqDistPoint2Segment(const Eigen::Vector2s& p, const Eigen::Vector2s& a, c
return sqDist;
}
-Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
- const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined, bool strict)
+double sqDistPointToLine(const Eigen::Vector3d& _A, const Eigen::Vector3d& _A_aux,
+ const Eigen::Vector3d& _B, bool _A_defined, bool _B_defined, bool strict)
{
/* Squared distance from B to the line A_aux-A (match evaluated is A-B)
*
@@ -129,9 +129,9 @@ Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_au
assert((!_A_defined || !_B_defined) && "ProcessorPolyline::sqDistPointToLine: at least one point must not be defined.");
- Scalar AB_sq = (_B-_A).head<2>().squaredNorm();
- Scalar AauxB_sq = (_B-_A_aux).head<2>().squaredNorm();
- Scalar AAaux_sq = (_A_aux-_A).head<2>().squaredNorm();
+ double AB_sq = (_B-_A).head<2>().squaredNorm();
+ double AauxB_sq = (_B-_A_aux).head<2>().squaredNorm();
+ double AAaux_sq = (_A_aux-_A).head<2>().squaredNorm();
// squared distance to line
// Case 1
@@ -165,10 +165,10 @@ Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_au
return 1e12;
}
-MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
- const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
- const Scalar& sq_error_max,
- const Scalar& overlap_dist_min,
+MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXd& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
+ const Eigen::MatrixXd& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
+ const double& sq_error_max,
+ const double& overlap_dist_min,
const int& overlap_N_min,
const int& overlap_N_defined_min,
bool both_exceeding_A_allowed_,
@@ -313,20 +313,20 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
// COND 2: sufficient overlapping defined points
// COND 3: sufficient overlapping distance
int N_defined_overlap = std::max(0,N_overlap - (from_A_defined && from_B_defined ? 0 : 1) - (to_A_defined && to_B_defined ? 0 : 1));
- Scalar overlap_dist;
+ double overlap_dist;
if (N_overlap <= 1)
overlap_dist = 0;
else
{
- Eigen::Vector2s p_from_A = _points_A.col(from_A).head(2);
- Eigen::Vector2s p_to_A = _points_A.col(to_A).head(2);
- Eigen::Vector2s p_from_B = _points_B.col(from_B).head(2);
- Eigen::Vector2s p_to_B = _points_B.col(to_B).head(2);
- Eigen::Vector2s vA = p_to_A - p_from_A;
- Eigen::Vector2s vB = p_to_B - p_from_B;
+ Eigen::Vector2d p_from_A = _points_A.col(from_A).head(2);
+ Eigen::Vector2d p_to_A = _points_A.col(to_A).head(2);
+ Eigen::Vector2d p_from_B = _points_B.col(from_B).head(2);
+ Eigen::Vector2d p_to_B = _points_B.col(to_B).head(2);
+ Eigen::Vector2d vA = p_to_A - p_from_A;
+ Eigen::Vector2d vB = p_to_B - p_from_B;
// THE MINIMUM OF ALL 6 possible overlap distances
- Eigen::Vector6s overlap_dists;
+ Eigen::Vector6d overlap_dists;
// 1: fromA - toA
overlap_dists(0) = vA.norm();
// 2: fromB - toB
@@ -371,7 +371,7 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
// NECESSARY COND: All squared distances should be lower than a threshold
// squared distances for all overlapping points
- Eigen::Array<Scalar,Eigen::Dynamic,1> dist2 = computeSqDist(_points_A, from_A, to_A, from_A_defined, to_A_defined,
+ Eigen::Array<double,Eigen::Dynamic,1> dist2 = computeSqDist(_points_A, from_A, to_A, from_A_defined, to_A_defined,
_points_B, from_B, to_B, from_B_defined, to_B_defined,
last_A, N_overlap );
if (print)
@@ -386,7 +386,7 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
}
// COMPUTE SCORE (distance based)
- Scalar normalized_score = (sq_error_max - dist2.mean()) / sq_error_max;
+ double normalized_score = (sq_error_max - dist2.mean()) / sq_error_max;
if (best_match != nullptr && print)
{
std::cout << "\tnormalized_score = " << normalized_score << std::endl;
@@ -417,8 +417,8 @@ MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _fir
return best_match;
}
-Eigen::Array<Scalar,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXs& _points_A, const int& from_A, const int& to_A, bool from_A_defined, bool to_A_defined,
- const Eigen::MatrixXs& _points_B, const int& from_B, const int& to_B, bool from_B_defined, bool to_B_defined,
+Eigen::Array<double,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXd& _points_A, const int& from_A, const int& to_A, bool from_A_defined, bool to_A_defined,
+ const Eigen::MatrixXd& _points_B, const int& from_B, const int& to_B, bool from_B_defined, bool to_B_defined,
const int& last_A, const int& N_overlap )
{
// POINT-POINT DISTANCES for all overlap points (in case of closed A in two parts)
@@ -433,7 +433,7 @@ Eigen::Array<Scalar,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXs& _poin
d.leftCols(_points_A.cols()-from_A) -= _points_A.block(0,from_A, 2, _points_A.cols()-from_A).array();
d.rightCols(to_A+1) -= _points_A.block(0, 0, 2, to_A+1).array();
}
- Eigen::Array<Scalar,Eigen::Dynamic,1> dist2 = d.row(0).pow(2) + d.row(1).pow(2);
+ Eigen::Array<double,Eigen::Dynamic,1> dist2 = d.row(0).pow(2) + d.row(1).pow(2);
// CORRECT POINT-LINE DISTANCES for not defined extremes
// First
@@ -473,9 +473,9 @@ Eigen::Array<Scalar,Eigen::Dynamic,1> computeSqDist(const Eigen::MatrixXs& _poin
return dist2;
}
-MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
- const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
- const Scalar& max_sq_error )
+MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXd& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
+ const Eigen::MatrixXd& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
+ const double& max_sq_error )
{
//std::cout << "computeBestRigidTrans...\n";
@@ -620,8 +620,8 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
int from_A_def = from_A_defined ? from_A : from_A+1;
int to_A_def = to_A_defined ? to_A : to_A+1;
int N_overlap_def = N_overlap - (from_B_defined && from_A_defined ? 0 : 1) - (to_B_defined && to_A_defined ? 0 : 1);
- Eigen::MatrixXs points_B_def = _points_B.middleCols(from_B_def, N_overlap_def);
- Eigen::MatrixXs points_A_def(_points_A.rows(), N_overlap_def);
+ Eigen::MatrixXd points_B_def = _points_B.middleCols(from_B_def, N_overlap_def);
+ Eigen::MatrixXd points_A_def(_points_A.rows(), N_overlap_def);
if (to_A >= from_A)
points_A_def = _points_A.middleCols(from_A_def, N_overlap_def);
else
@@ -632,11 +632,11 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
//std::cout << "\tpoints filled...\n";
//std::cout << "\tcomputeRigidTrans\n";
- Eigen::Vector3s pose_A_B = computeRigidTrans(points_A_def, points_B_def);
- Eigen::Matrix3s T_A_B = pose2T2D(pose_A_B.head(2), pose_A_B(2));
+ Eigen::Vector3d pose_A_B = computeRigidTrans(points_A_def, points_B_def);
+ Eigen::Matrix3d T_A_B = pose2T2D(pose_A_B.head(2), pose_A_B(2));
// POINT-POINT DISTANCES of the matching of defined points
- Eigen::MatrixXs d = (points_A_def - T_A_B * points_B_def).topRows(2);
+ Eigen::MatrixXd d = (points_A_def - T_A_B * points_B_def).topRows(2);
Eigen::ArrayXd dist2(N_overlap);
dist2.middleRows(from_B_defined && from_A_defined ? 0 : 1, N_overlap_def) = d.colwise().squaredNorm().transpose().array();
@@ -700,7 +700,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
return matches;
}
-bool isProjectedOverSegment(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B)
+bool isProjectedOverSegment(const Eigen::Vector3d& _A, const Eigen::Vector3d& _A_aux, const Eigen::Vector3d& _B)
{
/* The orthogonal projection of B over the line A-Aaux is in the segment [A,Aaux].
*
@@ -709,25 +709,25 @@ bool isProjectedOverSegment(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A
*
*/
- Scalar AB_sq = (_B-_A).head<2>().squaredNorm();
- Scalar AauxB_sq = (_B-_A_aux).head<2>().squaredNorm();
- Scalar AAaux_sq = (_A_aux-_A).head<2>().squaredNorm();
+ double AB_sq = (_B-_A).head<2>().squaredNorm();
+ double AauxB_sq = (_B-_A_aux).head<2>().squaredNorm();
+ double AAaux_sq = (_A_aux-_A).head<2>().squaredNorm();
return (AauxB_sq <= AB_sq + AAaux_sq) && (AB_sq <= AauxB_sq + AAaux_sq);
}
-Eigen::VectorXs computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
- const Eigen::Matrix2s& _feature_cov,
- const Eigen::Vector2s& _expected_feature,
- const Eigen::Matrix2s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu)
+Eigen::VectorXd computeSquaredMahalanobisDistances(const Eigen::Vector2d& _feature,
+ const Eigen::Matrix2d& _feature_cov,
+ const Eigen::Vector2d& _expected_feature,
+ const Eigen::Matrix2d& _expected_feature_cov,
+ const Eigen::MatrixXd& _mu)
{
// ------------------------ d
- Eigen::Vector2s d = _feature - _expected_feature;
+ Eigen::Vector2d d = _feature - _expected_feature;
// ------------------------ Sigma_d
- Eigen::Matrix2s iSigma_d = (_feature_cov + _expected_feature_cov).inverse();
- Eigen::VectorXs squared_mahalanobis_distances(_mu.cols());
+ Eigen::Matrix2d iSigma_d = (_feature_cov + _expected_feature_cov).inverse();
+ Eigen::VectorXd squared_mahalanobis_distances(_mu.cols());
for (unsigned int i = 0; i < _mu.cols(); i++)
{
squared_mahalanobis_distances(i) = (d - _mu.col(i)).transpose() * iSigma_d * (d - _mu.col(i));
diff --git a/src/processor/processor_closeloop_icp.cpp b/src/processor/processor_closeloop_icp.cpp
index 721619894428beab720720aa312fea2c8ca9ddbd..8e5417f4cf92817f25d9acd8671e0a835db070da 100644
--- a/src/processor/processor_closeloop_icp.cpp
+++ b/src/processor/processor_closeloop_icp.cpp
@@ -76,7 +76,7 @@ void ProcessorCloseLoopICP::processCapture(CaptureBasePtr _capture_ptr)
// laserscanutils::LaserScan& last_scan = cpt_ptr->getScan();
// laserscanutils::LaserScan& origin_scan = _origin_cpt->getScan();
// std::cout << "Number of ranges: " << cpt_ptr->getScan().ranges_raw_.size() << '\n';
- // Eigen::Vector3s transform_0;
+ // Eigen::Vector3d transform_0;
// transform_0 << 0,0,0;
// laserscanutils::icp_output result = laserscanutils::ICP::matchPC(last_scan, origin_scan, scan_params, transform_0);
// std::cout << "We made it past result " << std::endl;
diff --git a/src/processor/processor_odom_icp.cpp b/src/processor/processor_odom_icp.cpp
index ad009aa269ee3b196335ea7e16d8037ba6dfdd43..653f3d75073289116ed486879966193c81d943e6 100644
--- a/src/processor/processor_odom_icp.cpp
+++ b/src/processor/processor_odom_icp.cpp
@@ -47,12 +47,12 @@ ProcessorOdomICP::ProcessorOdomICP(ProcessorParamsOdomICPPtr _params):
icp_tools_ptr_ = std::make_shared<ICP>();
// Frame transforms
- trf_origin_last_.res_transf = Eigen::Vector3s::Zero();
- trf_origin_incoming_.res_transf = Eigen::Vector3s::Zero();
- trf_last_incoming_.res_transf = Eigen::Vector3s::Zero();
- trf_origin_last_.res_covar = Eigen::Matrix3s::Identity();
- trf_origin_incoming_.res_covar = Eigen::Matrix3s::Identity();
- trf_last_incoming_.res_covar = Eigen::Matrix3s::Identity();
+ trf_origin_last_.res_transf = Eigen::Vector3d::Zero();
+ trf_origin_incoming_.res_transf = Eigen::Vector3d::Zero();
+ trf_last_incoming_.res_transf = Eigen::Vector3d::Zero();
+ trf_origin_last_.res_covar = Eigen::Matrix3d::Identity();
+ trf_origin_incoming_.res_covar = Eigen::Matrix3d::Identity();
+ trf_last_incoming_.res_covar = Eigen::Matrix3d::Identity();
}
ProcessorOdomICP::~ProcessorOdomICP()
@@ -115,9 +115,9 @@ unsigned int ProcessorOdomICP::processKnown()
this->icp_params_,
this->trf_origin_last_.res_transf); // Check order
- // Scalar score_normalized
- // ((Scalar)trf_origin_incoming_.nvalid / (Scalar)min(incoming_ptr->getScan().ranges_raw_.size(), origin_ptr->getScan().ranges_raw_.size()));
- // Scalar mean_error = trf_origin_incoming_.error / trf_origin_incoming_.nvalid;
+ // double score_normalized
+ // ((double)trf_origin_incoming_.nvalid / (double)min(incoming_ptr->getScan().ranges_raw_.size(), origin_ptr->getScan().ranges_raw_.size()));
+ // double mean_error = trf_origin_incoming_.error / trf_origin_incoming_.nvalid;
// WOLF_INFO("DBG ------------------------------");
// WOLF_INFO("DBG valid? ", trf_origin_incoming_.valid, " m_er ", mean_error, " ", score_normalized * 100, "% own_id: ", incoming_ptr->id(),
// " other_id: ", origin_ptr->id());
@@ -131,7 +131,7 @@ unsigned int ProcessorOdomICP::processKnown()
unsigned int ProcessorOdomICP::processNew(const int& _max_features)
{
- Eigen::Vector3s t_identity;
+ Eigen::Vector3d t_identity;
t_identity << 0, 0, 0;
// XXX: Dynamic or static? JS: static is OK.
@@ -158,8 +158,8 @@ unsigned int ProcessorOdomICP::processNew(const int& _max_features)
WOLF_TRACE("trf_last_incoming_: ", trf_last_incoming_.res_transf.transpose());
Eigen::Matrix2s R_last;
- Scalar alphal = trf_origin_last_.res_transf(2);
- Scalar alphali = trf_origin_incoming_.res_transf(2) - alphal;
+ double alphal = trf_origin_last_.res_transf(2);
+ double alphali = trf_origin_incoming_.res_transf(2) - alphal;
R_last << cos(alphal), -sin(alphal), sin(alphal), cos(alphal);
Eigen::Vector3s trf_last_incoming_check;
trf_last_incoming_check.head(2) = R_last.transpose() * (trf_origin_incoming_.res_transf.head(2) - trf_origin_last_.res_transf.head(2));
@@ -203,7 +203,7 @@ inline bool ProcessorOdomICP::voteForKeyFrameAngle() const
inline bool ProcessorOdomICP::voteForKeyFrameTime() const
{
- Scalar secs = incoming_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp();
+ double secs = incoming_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp();
bool vote = (secs > proc_params_->vfk_min_time);
@@ -235,15 +235,15 @@ void ProcessorOdomICP::advanceDerived()
// overwrite last frame's state
- Isometry2ds w_T_ro = Translation2ds(origin_ptr_->getFrame()->getState().head(2)) * Rotation2Ds(origin_ptr_->getFrame()->getState()(2));
- Isometry2ds ro_T_so = Translation2ds(origin_ptr_->getSensorP()->getState()) * Rotation2Ds(origin_ptr_->getSensorO()->getState()(0));
- Isometry2ds& ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
+ Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getState().head(2)) * Rotation2Dd(origin_ptr_->getFrame()->getState()(2));
+ Isometry2d ro_T_so = Translation2d(origin_ptr_->getSensorP()->getState()) * Rotation2Dd(origin_ptr_->getSensorO()->getState()(0));
+ Isometry2d& ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
// incoming
- Isometry2ds so_T_si = Translation2ds(trf_origin_incoming_.res_transf.head(2)) * Rotation2Ds(trf_origin_incoming_.res_transf(2));
- Isometry2ds w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
- Vector3s x_inc; x_inc << w_T_ri.translation() , Rotation2Ds(w_T_ri.rotation()).angle();
+ Isometry2d so_T_si = Translation2d(trf_origin_incoming_.res_transf.head(2)) * Rotation2Dd(trf_origin_incoming_.res_transf(2));
+ Isometry2d w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
+ Vector3d x_inc; x_inc << w_T_ri.translation() , Rotation2Dd(w_T_ri.rotation()).angle();
// Put the state of incoming in last
last_ptr_->getFrame()->setState(x_inc);
@@ -272,50 +272,18 @@ void ProcessorOdomICP::resetDerived()
// x1_p_y1_y2: translation vector from y1 to y2 expressed in frame x1
- Isometry2ds w_T_ro = Translation2ds(origin_ptr_->getFrame()->getState().head(2)) * Rotation2Ds(origin_ptr_->getFrame()->getState()(2));
- Isometry2ds ro_T_so = Translation2ds(origin_ptr_->getSensorP()->getState()) * Rotation2Ds(origin_ptr_->getSensorO()->getState()(0));
- Isometry2ds &ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
+ Isometry2d w_T_ro = Translation2d(origin_ptr_->getFrame()->getState().head(2)) * Rotation2Dd(origin_ptr_->getFrame()->getState()(2));
+ Isometry2d ro_T_so = Translation2d(origin_ptr_->getSensorP()->getState()) * Rotation2Dd(origin_ptr_->getSensorO()->getState()(0));
+ Isometry2d &ri_T_si = ro_T_so; // A reference just to have nice names without computing overhead
// incoming
- Isometry2ds so_T_si = Translation2ds(trf_origin_incoming_.res_transf.head(2)) * Rotation2Ds(trf_origin_incoming_.res_transf(2));
- Isometry2ds w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
- Vector3s x_current;
- x_current << w_T_ri.translation(), Rotation2Ds(w_T_ri.rotation()).angle();
+ Isometry2d so_T_si = Translation2d(trf_origin_incoming_.res_transf.head(2)) * Rotation2Dd(trf_origin_incoming_.res_transf(2));
+ Isometry2d w_T_ri = w_T_ro * ro_T_so * so_T_si * ri_T_si.inverse();
+ Vector3d x_current;
+ x_current << w_T_ri.translation(), Rotation2Dd(w_T_ri.rotation()).angle();
- // Isometry2ds w_T_ro = Translation2ds(origin_ptr_->getFrame()->getState().head(2)) * Rotation2Ds(origin_ptr_->getFrame()->getState()(2));
- // Isometry2ds ro_T_so = Translation2ds(origin_ptr_->getSensorP()->getState()) * Rotation2Ds(origin_ptr_->getSensorO()->getState()(0));
- // Isometry2ds &rl_T_sl = ro_T_so; // A reference just to have nice names without computing overhead
-
- // // last
-
- // Isometry2ds so_T_sl = Translation2ds(trf_origin_last_.res_transf.head(2)) * Rotation2Ds(trf_origin_last_.res_transf(2));
- // Isometry2ds w_T_rl = w_T_ro * ro_T_so * so_T_sl * rl_T_sl.inverse();
- // Vector3s x_current;
- // x_current << w_T_rl.translation(), Rotation2Ds(w_T_rl.rotation()).angle();
-
- // Rotation composition
- // Eigen::Rotation2D<Scalar> w_R_ro = Eigen::Rotation2D<Scalar>(origin_ptr_->getFrame()->getState()(2));
- // Eigen::Rotation2D<Scalar> r_R_s = Eigen::Rotation2D<Scalar>(origin_ptr_->getSensorO()->getState()(0));
- // Eigen::Rotation2D<Scalar> &ro_R_so = r_R_s; // just a ref for bettter chaining trf. names
- // Eigen::Rotation2D<Scalar> so_R_sl = Eigen::Rotation2D<Scalar>(trf_origin_last_.res_transf(2));
-
- // Eigen::Rotation2D<Scalar> w_R_rl = w_R_ro*r_R_s*so_R_sl*r_R_s.inverse();
- // ro_R_so = rl_R_sl = r_R_s
- // Planar rotations are commutative
- // Eigen::Rotation2D<Scalar> w_R_rl = w_R_ro * so_R_sl;
-
- // Translation composition
- // Eigen::Vector2s w_p_w_ro = origin_ptr_->getFrame()->getState().head(2);
- // Eigen::Vector2s w_p_ro_so = w_R_ro * origin_ptr_->getSensorP()->getState();
- // Eigen::Vector2s w_p_so_sl = w_R_ro * ro_R_so * trf_origin_last_.res_transf.head(2);
- // Eigen::Vector2s w_p_sl_rl = w_R_rl * (-last_ptr_->getSensorP()->getState());
-
- // Eigen::Vector2s w_p_w_rl = w_p_w_ro + w_p_ro_so + w_p_so_sl + w_p_sl_rl;
-
- // Eigen::Vector3s curr_state;
- // curr_state.head(2) = w_p_w_rl;
- // curr_state(2) = origin_ptr_->getFrame()->getState()(2) + trf_origin_last_.res_transf(2);
+ // last
last_ptr_->getFrame()->setState(x_current);
// incoming_ptr_->getFrame()->setState(x_current);
diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index 1edad9f4ae6276b3c73ef34a62226bbd4de8ee59..4c1630bb91122056ddada95d9dfcac3ef7db297e 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -34,7 +34,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseP
return 0;
// prior transformations
- Eigen::Matrix3s T_last_incoming_prior(Eigen::Matrix3s::Identity());
+ Eigen::Matrix3d T_last_incoming_prior(Eigen::Matrix3d::Identity());
T_last_incoming_prior.topLeftCorner(2,2) = R_last_incoming_;
T_last_incoming_prior.topRightCorner(2,1) = t_last_incoming_;
@@ -234,11 +234,11 @@ bool ProcessorTrackerFeaturePolyline2D::tryCompletePartialMatch(LandmarkMatchPol
auto exceeding_lmk = pl_lmk->getPrevValidId(lmk_inc_match->landmark_from_id_);
// check good overlap
- Eigen::Vector3s lmk_point_expected(Eigen::Vector3s::Ones());
+ Eigen::Vector3d lmk_point_expected(Eigen::Vector3d::Ones());
lmk_point_expected.head(2) = pl_lmk->getPointVector(exceeding_lmk);
- Eigen::Vector3s ftr_point = pl_inc->getPoints().col(exceeding_inc);
+ Eigen::Vector3d ftr_point = pl_inc->getPoints().col(exceeding_inc);
- Scalar sq_dist = (lmk_point_expected.head(2) - ftr_point.head(2)).squaredNorm();
+ double sq_dist = (lmk_point_expected.head(2) - ftr_point.head(2)).squaredNorm();
// exceeding point not matched -> return not succeed
if (sq_dist > params_tracker_feature_polyline_->match_landmark_position_sq_norm_max)
@@ -262,11 +262,11 @@ bool ProcessorTrackerFeaturePolyline2D::tryCompletePartialMatch(LandmarkMatchPol
auto exceeding_lmk = pl_lmk->getNextValidId(lmk_inc_match->landmark_to_id_);
// check good overlap
- Eigen::Vector3s lmk_point_expected(Eigen::Vector3s::Ones());
+ Eigen::Vector3d lmk_point_expected(Eigen::Vector3d::Ones());
lmk_point_expected.head(2) = pl_lmk->getPointVector(exceeding_lmk);
- Eigen::Vector3s ftr_point = pl_inc->getPoints().col(exceeding_inc);
+ Eigen::Vector3d ftr_point = pl_inc->getPoints().col(exceeding_inc);
- Scalar sq_dist = (lmk_point_expected.head(2) - ftr_point.head(2)).squaredNorm();
+ double sq_dist = (lmk_point_expected.head(2) - ftr_point.head(2)).squaredNorm();
// exceeding point not matched -> return not succeed
if (sq_dist > params_tracker_feature_polyline_->match_landmark_position_sq_norm_max)
@@ -308,7 +308,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
// WOLF_DEBUG("Processing ", n, " new last features");
// prior transformations
- Eigen::Matrix3s T_sensor_world_last(Eigen::Matrix3s::Identity());
+ Eigen::Matrix3d T_sensor_world_last(Eigen::Matrix3d::Identity());
T_sensor_world_last.topLeftCorner(2,2) = R_sensor_world_last_;
T_sensor_world_last.topRightCorner(2,1) = t_sensor_world_last_;
@@ -348,7 +348,7 @@ unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const int& _max_featu
new_lmk_match->feature_to_id_= pl_ftr->getNPoints()-1; // all points match
new_lmk_match->landmark_to_id_ = pl_ftr->getNPoints()-1; // all points match
new_lmk_match->normalized_score_ = 1.0; // max score
- new_lmk_match->T_feature_landmark_ = Eigen::Matrix3s::Identity();
+ new_lmk_match->T_feature_landmark_ = Eigen::Matrix3d::Identity();
new_lmk_match->T_feature_landmark_.topLeftCorner(2,2) = R_sensor_world_last_;
new_lmk_match->T_feature_landmark_.topRightCorner(2,1) = t_sensor_world_last_;
@@ -604,8 +604,8 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::emplaceLandmark(FeatureBasePt
FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr);
// compute feature global pose
- Eigen::MatrixXs points_global = R_world_sensor_last_ * polyline_ptr->getPoints().topRows<2>() +
- t_world_sensor_last_ * Eigen::VectorXs::Ones(polyline_ptr->getNPoints()).transpose();
+ Eigen::MatrixXd points_global = R_world_sensor_last_ * polyline_ptr->getPoints().topRows<2>() +
+ t_world_sensor_last_ * Eigen::VectorXd::Ones(polyline_ptr->getNPoints()).transpose();
// std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl;
// std::cout << "Landmark global points: " << std::endl << points_global << std::endl;
@@ -613,8 +613,8 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::emplaceLandmark(FeatureBasePt
// Create new landmark
return LandmarkBase::emplace<LandmarkPolyline2D>(getProblem()->getMap(),
- std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true),
- std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true),
+ std::make_shared<StateBlock>(Eigen::Vector2d::Zero(), true),
+ std::make_shared<StateBlock>(Eigen::Vector1d::Zero(), true),
points_global,
polyline_ptr->isFirstDefined(),
polyline_ptr->isLastDefined());
@@ -632,9 +632,9 @@ bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPoly
assert(lmk_match->check() && "Bad landmark match provided");
- Eigen::MatrixXs points_global2 = R_world_sensor_last_ * pl_ftr->getPoints().topRows<2>() +
- t_world_sensor_last_ * Eigen::VectorXs::Ones(pl_ftr->getNPoints()).transpose();
- Eigen::MatrixXs points_global = lmk_match->T_feature_landmark_.inverse() * pl_ftr->getPoints();
+ Eigen::MatrixXd points_global2 = R_world_sensor_last_ * pl_ftr->getPoints().topRows<2>() +
+ t_world_sensor_last_ * Eigen::VectorXd::Ones(pl_ftr->getNPoints()).transpose();
+ Eigen::MatrixXd points_global = lmk_match->T_feature_landmark_.inverse() * pl_ftr->getPoints();
if (print)
{
std::cout << "landmark points" << std::endl << pl_lmk->computePointsGlobal() << std::endl;
@@ -868,7 +868,7 @@ void ProcessorTrackerFeaturePolyline2D::preProcess()
//WOLF_DEBUG("new polyline detected: Defined", pl.first_defined_ , pl.last_defined_ );
//std::cout << "covs: " << std::endl << pl.covs_ << std::endl;
auto points = pl.points_;
- points.bottomRows(1) = Eigen::MatrixXs::Ones(1,points.cols());
+ points.bottomRows(1) = Eigen::MatrixXd::Ones(1,points.cols());
untracked_features_incoming_.push_back(std::make_shared<FeaturePolyline2D>(points, pl.covs_, pl.first_defined_, pl.last_defined_));
//WOLF_DEBUG("new polyline detected: points\n",pl.points_);
}
@@ -934,7 +934,7 @@ void ProcessorTrackerFeaturePolyline2D::postProcess()
void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
const FeaturePolyline2DPtr& _ftr_L,
const FeaturePolyline2DPtr& _ftr_I,
- const Eigen::Matrix3s& _T_last_incoming_prior) const
+ const Eigen::Matrix3d& _T_last_incoming_prior) const
{
//WOLF_DEBUG("tryMatchWithFeature: incoming ", _ftr_I->id(), " last ", _ftr_L->id());
@@ -943,24 +943,24 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline
if (params_tracker_feature_polyline_->use_probabilistic_match)
{
// Compute Covariance of projected points (only considering noise in T_last_incoming) and computing Jacobian using the mean of the points
- Eigen::Vector2s mean_last_points = _ftr_L->getPoints().rowwise().mean().head(2);
- Eigen::Vector2s J_th;
- Scalar cos_th = _T_last_incoming_prior(0,0);
- Scalar sin_th = _T_last_incoming_prior(1,0);
+ Eigen::Vector2d mean_last_points = _ftr_L->getPoints().rowwise().mean().head(2);
+ Eigen::Vector2d J_th;
+ double cos_th = _T_last_incoming_prior(0,0);
+ double sin_th = _T_last_incoming_prior(1,0);
J_th << -sin_th * mean_last_points(0) - cos_th * mean_last_points(1),
cos_th * mean_last_points(0) - sin_th * mean_last_points(1);
- Eigen::Matrix2s Sigma = Eigen::Matrix2s::Identity()*params_tracker_feature_polyline_->match_feature_position_sq_norm_max +
+ Eigen::Matrix2d Sigma = Eigen::Matrix2d::Identity()*params_tracker_feature_polyline_->match_feature_position_sq_norm_max +
J_th*params_tracker_feature_polyline_->match_feature_orientation_sq_norm_max*J_th.transpose();
// sq root of inverse
- Eigen::Matrix2s sqrtOmega = Eigen::Matrix2s(Sigma.llt().matrixL()).inverse();
+ Eigen::Matrix2d sqrtOmega = Eigen::Matrix2d(Sigma.llt().matrixL()).inverse();
// last expected points
- Eigen::Matrix<Scalar, 3, Eigen::Dynamic> last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
+ Eigen::Matrix<double, 3, Eigen::Dynamic> last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
// Shape points in the equiprobabilistic space
- Eigen::Matrix<Scalar, 3, Eigen::Dynamic> last_shaped = last_expected_points;
- Eigen::Matrix<Scalar, 3, Eigen::Dynamic> inc_shaped = _ftr_I->getPoints();
+ Eigen::Matrix<double, 3, Eigen::Dynamic> last_shaped = last_expected_points;
+ Eigen::Matrix<double, 3, Eigen::Dynamic> inc_shaped = _ftr_I->getPoints();
last_shaped.topRows<2>() = sqrtOmega*last_shaped.topRows<2>();
inc_shaped.topRows<2>() = sqrtOmega*inc_shaped.topRows<2>();
@@ -981,7 +981,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline
else // not probabilistic match
{
// last expected points
- Eigen::Matrix<Scalar, 3, Eigen::Dynamic> last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
+ Eigen::Matrix<double, 3, Eigen::Dynamic> last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
// compute best sq Mahalanobis distance matching
best_match = computeBestSqDist(_ftr_I->getPoints(), // <--feature incoming points
@@ -1042,7 +1042,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline
void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
const LandmarkPolyline2DPtr& _lmk_ptr,
const FeaturePolyline2DPtr& _feat_ptr,
- const Eigen::Matrix3s& _T_feature_landmark_prior) const
+ const Eigen::Matrix3d& _T_feature_landmark_prior) const
{
//WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithLandmark: ");
@@ -1051,24 +1051,24 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
if (params_tracker_feature_polyline_->use_probabilistic_match)
{
// Compute Covariance of projected points (only considering noise in _T_feature_landmark_prior) and computing Jacobian using the mean of the points
- Eigen::Vector2s mean_last_points = _lmk_ptr->computePointsGlobal().rowwise().mean().head(2);
- Eigen::Vector2s J_th;
- Scalar cos_th = _T_feature_landmark_prior(0,0);
- Scalar sin_th = _T_feature_landmark_prior(1,0);
+ Eigen::Vector2d mean_last_points = _lmk_ptr->computePointsGlobal().rowwise().mean().head(2);
+ Eigen::Vector2d J_th;
+ double cos_th = _T_feature_landmark_prior(0,0);
+ double sin_th = _T_feature_landmark_prior(1,0);
J_th << -sin_th * mean_last_points(0) - cos_th * mean_last_points(1),
cos_th * mean_last_points(0) - sin_th * mean_last_points(1);
- Eigen::Matrix2s Sigma = Eigen::Matrix2s::Identity()*params_tracker_feature_polyline_->match_landmark_position_sq_norm_max +
+ Eigen::Matrix2d Sigma = Eigen::Matrix2d::Identity()*params_tracker_feature_polyline_->match_landmark_position_sq_norm_max +
J_th*params_tracker_feature_polyline_->match_landmark_orientation_sq_norm_max*J_th.transpose();
// sq root of inverse
- Eigen::Matrix2s sqrtOmega = Eigen::Matrix2s(Sigma.llt().matrixL()).inverse();
+ Eigen::Matrix2d sqrtOmega = Eigen::Matrix2d(Sigma.llt().matrixL()).inverse();
// landmark expected points
- Eigen::MatrixXs lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
+ Eigen::MatrixXd lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
// Shape points in the equiprobabilistic space
- Eigen::Matrix<Scalar, 3, Eigen::Dynamic> lmk_shaped = lmk_expected_points;
- Eigen::Matrix<Scalar, 3, Eigen::Dynamic> feat_shaped = _feat_ptr->getPoints();
+ Eigen::Matrix<double, 3, Eigen::Dynamic> lmk_shaped = lmk_expected_points;
+ Eigen::Matrix<double, 3, Eigen::Dynamic> feat_shaped = _feat_ptr->getPoints();
lmk_shaped.topRows<2>() = sqrtOmega*lmk_shaped.topRows<2>();
feat_shaped.topRows<2>() = sqrtOmega*feat_shaped.topRows<2>();
@@ -1091,7 +1091,7 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
else // not probabilistic match
{
// landmark expected points
- Eigen::MatrixXs lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
+ Eigen::MatrixXd lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
// compute best sq distance matching
best_match = computeBestSqDist(lmk_expected_points, // <--landmark points in local coordinates
@@ -1300,15 +1300,15 @@ bool ProcessorTrackerFeaturePolyline2D::tryUpdateMatchTransformation(LandmarkMat
void ProcessorTrackerFeaturePolyline2D::computeTransformations()
{
//WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations: ");
- Eigen::Vector3s vehicle_pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp());
- Eigen::Vector3s vehicle_pose_last = getProblem()->getState(last_ptr_->getTimeStamp());
+ Eigen::Vector3d vehicle_pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp());
+ Eigen::Vector3d vehicle_pose_last = getProblem()->getState(last_ptr_->getTimeStamp());
//WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations:",
// "\n\tvehicle_pose_incoming: ", vehicle_pose_incoming.transpose(),
// "\n\tvehicle_pose_last: ", vehicle_pose_last.transpose());
// world_robot
- Eigen::Matrix2s R_world_robot_incoming = Eigen::Rotation2Ds(vehicle_pose_incoming(2)).matrix();
- Eigen::Matrix2s R_world_robot_last = Eigen::Rotation2Ds(vehicle_pose_last(2)).matrix();
+ Eigen::Matrix2d R_world_robot_incoming = Eigen::Rotation2Dd(vehicle_pose_incoming(2)).matrix();
+ Eigen::Matrix2d R_world_robot_last = Eigen::Rotation2Dd(vehicle_pose_last(2)).matrix();
// robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
if (getSensor()->isStateBlockDynamic("P") ||
@@ -1318,7 +1318,7 @@ void ProcessorTrackerFeaturePolyline2D::computeTransformations()
!extrinsics_transformation_computed_)
{
t_robot_sensor_ = getSensor()->getP()->getState();
- R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix();
+ R_robot_sensor_ = Eigen::Rotation2Dd(getSensor()->getO()->getState()(0)).matrix();
extrinsics_transformation_computed_ = true;
}
diff --git a/src/sensor/sensor_laser_2D.cpp b/src/sensor/sensor_laser_2D.cpp
index a49b7636a951c5575dd23cec348466b6b70800db..9312c5b2be313e214a304e617e2c3ccc725dd3e4 100644
--- a/src/sensor/sensor_laser_2D.cpp
+++ b/src/sensor/sensor_laser_2D.cpp
@@ -39,8 +39,13 @@ SensorLaser2D::SensorLaser2D(StateBlockPtr _p_ptr, StateBlockPtr _o_ptr, const I
//
}
-SensorLaser2D::SensorLaser2D(const Eigen::VectorXs& _extrinsics, const IntrinsicsLaser2DPtr _params) :
+//<<<<<<< HEAD
+SensorLaser2D::SensorLaser2D(const Eigen::VectorXd& _extrinsics, const IntrinsicsLaser2DPtr _params) :
SensorBase("SensorLaser2D",
+//=======
+//SensorLaser2D::SensorLaser2D(const Eigen::VectorXd& _extrinsics, const IntrinsicsLaser2DPtr _params) :
+// SensorBase("LASER 2D",
+//>>>>>>> 8-replace-scalar-to-double
std::make_shared<StateBlock>(_extrinsics.head(2), false),
std::make_shared<StateAngle>(_extrinsics(2), false),
nullptr,
@@ -65,7 +70,7 @@ void SensorLaser2D::setDefaultScanParams()
scan_params_.range_max_ = 100;
scan_params_.range_std_dev_ = 0.01;
- setNoiseStd(Eigen::VectorXs::Constant(1,scan_params_.range_std_dev_));
+ setNoiseStd(Eigen::VectorXd::Constant(1,scan_params_.range_std_dev_));
}
void SensorLaser2D::setScanParams(const laserscanutils::LaserScanParams & _params)
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_polyline_extreme.cpp b/src/state_block/local_parametrization_polyline_extreme.cpp
index cdf3020e9e4c0b4c26d713f38e295284d593f777..741c708a8f9c4f7e08bcd768456aca0d71cf25b4 100644
--- a/src/state_block/local_parametrization_polyline_extreme.cpp
+++ b/src/state_block/local_parametrization_polyline_extreme.cpp
@@ -14,22 +14,22 @@ LocalParametrizationPolylineExtreme::~LocalParametrizationPolylineExtreme()
{
}
-bool LocalParametrizationPolylineExtreme::plus(Eigen::Map<const Eigen::VectorXs>& _point,
- Eigen::Map<const Eigen::VectorXs>& _delta_theta,
- Eigen::Map<Eigen::VectorXs>& _point_plus_delta_theta) const
+bool LocalParametrizationPolylineExtreme::plus(Eigen::Map<const Eigen::VectorXd>& _point,
+ Eigen::Map<const Eigen::VectorXd>& _delta_theta,
+ Eigen::Map<Eigen::VectorXd>& _point_plus_delta_theta) const
{
assert(_point.size() == global_size_ && "Wrong size of input point.");
assert(_delta_theta.size() == local_size_ && "Wrong size of input delta_theta.");
assert(_point_plus_delta_theta.size() == global_size_ && "Wrong size of output quaternion.");
- _point_plus_delta_theta = reference_point_->getState().head(2) + Eigen::Rotation2Ds(_delta_theta(0)) * (_point - reference_point_->getState().head(2));
+ _point_plus_delta_theta = reference_point_->getState().head(2) + Eigen::Rotation2Dd(_delta_theta(0)) * (_point - reference_point_->getState().head(2));
return true;
}
-bool LocalParametrizationPolylineExtreme::computeJacobian(Eigen::Map<const Eigen::VectorXs>& _point,
- Eigen::Map<Eigen::MatrixXs>& _jacobian) const
+bool LocalParametrizationPolylineExtreme::computeJacobian(Eigen::Map<const Eigen::VectorXd>& _point,
+ Eigen::Map<Eigen::MatrixXd>& _jacobian) const
{
assert(_point.size() == global_size_ && "Wrong size of input point.");
assert(_jacobian.rows() == global_size_ && _jacobian.cols() == local_size_ && "Wrong size of Jacobian matrix.");
@@ -40,12 +40,12 @@ bool LocalParametrizationPolylineExtreme::computeJacobian(Eigen::Map<const Eigen
return true;
}
-bool LocalParametrizationPolylineExtreme::minus(Eigen::Map<const Eigen::VectorXs>& _point1,
- Eigen::Map<const Eigen::VectorXs>& _point2,
- Eigen::Map<Eigen::VectorXs>& _delta_theta)
+bool LocalParametrizationPolylineExtreme::minus(Eigen::Map<const Eigen::VectorXd>& _point1,
+ Eigen::Map<const Eigen::VectorXd>& _point2,
+ Eigen::Map<Eigen::VectorXd>& _delta_theta)
{
- Eigen::Vector2s v1 = _point1 - reference_point_->getState().head(2);
- Eigen::Vector2s v2 = _point2 - reference_point_->getState().head(2);
+ Eigen::Vector2d v1 = _point1 - reference_point_->getState().head(2);
+ Eigen::Vector2d v2 = _point2 - reference_point_->getState().head(2);
_delta_theta(0) = pi2pi(atan2(v2(1),v2(0)) - atan2(v1(1),v1(0)));
diff --git a/src/yaml/processor_odom_ICP_yaml.cpp b/src/yaml/processor_odom_ICP_yaml.cpp
index e38342e212c85fc92966435765a0af261b65fb76..e93cf3984bc50a1db7c1d696cbb5f4ceabc4358b 100644
--- a/src/yaml/processor_odom_ICP_yaml.cpp
+++ b/src/yaml/processor_odom_ICP_yaml.cpp
@@ -32,23 +32,23 @@ ProcessorParamsBasePtr createProcessorParamsOdomICP(const std::string& _filename
params->voting_active = config["voting_active"] .as<bool>();
params->voting_aux_active = config["voting_aux_active"] .as<bool>();
- params->time_tolerance = config["time_tolerance"] .as<Scalar>();
+ params->time_tolerance = config["time_tolerance"] .as<double>();
- params->min_features_for_keyframe = config["min_features_for_keyframe"] .as<Scalar>();
+ params->min_features_for_keyframe = config["min_features_for_keyframe"] .as<double>();
params->max_new_features = config["max_new_features"] .as<SizeEigen>();
- params->use_point_to_line_distance = config["use_point_to_line_distance"] .as<int>();
- params->max_correspondence_dist = config["max_correspondence_dist"] .as<int>();
- params->max_iterations = config["max_iterations"] .as<int>();
- params->use_corr_tricks = config["use_corr_tricks"] .as<int>();
- params->outliers_maxPerc = config["outliers_maxPerc"] .as<Scalar>();
- params->outliers_adaptive_order = config["outliers_adaptive_order"] .as<Scalar>();
- params->outliers_adaptive_mult = config["outliers_adaptive_mult"] .as<Scalar>();
- params->vfk_min_dist = config["vfk_min_dist"] .as<Scalar>();
- params->vfk_min_angle = config["vfk_min_angle"] .as<Scalar>();
- params->vfk_min_time = config["vfk_min_time"] .as<Scalar>();
- params->vfk_min_error = config["vfk_min_error"] .as<Scalar>();
- params->vfk_max_points = config["vfk_max_points"] .as<int>();
+ params->use_point_to_line_distance = config["use_point_to_line_distance"] .as<int>();
+ params->max_correspondence_dist = config["max_correspondence_dist"] .as<int>();
+ params->max_iterations = config["max_iterations"] .as<int>();
+ params->use_corr_tricks = config["use_corr_tricks"] .as<int>();
+ params->outliers_maxPerc = config["outliers_maxPerc"] .as<double>();
+ params->outliers_adaptive_order = config["outliers_adaptive_order"] .as<double>();
+ params->outliers_adaptive_mult = config["outliers_adaptive_mult"] .as<double>();
+ params->vfk_min_dist = config["vfk_min_dist"] .as<double>();
+ params->vfk_min_angle = config["vfk_min_angle"] .as<double>();
+ params->vfk_min_time = config["vfk_min_time"] .as<double>();
+ params->vfk_min_error = config["vfk_min_error"] .as<double>();
+ params->vfk_max_points = config["vfk_max_points"] .as<int>();
return params;
}
diff --git a/test/gtest_landmark_polyline.cpp b/test/gtest_landmark_polyline.cpp
index 4accc8b55f898322002b7788bed4a97965e86084..6bf84ca5e5afdf249fddb8e8bdd70743fcdb68e0 100644
--- a/test/gtest_landmark_polyline.cpp
+++ b/test/gtest_landmark_polyline.cpp
@@ -10,8 +10,8 @@ class LandmarkPolylineTest : public testing::Test
LandmarkPolyline2DPtr lmk_ptr;
std::vector<LandmarkPolyline2DPtr> lmks_class_list;
- std::vector<VectorXs> lmks_groundtruth;
- std::vector<MatrixXs> points_groundtruth;
+ std::vector<VectorXd> lmks_groundtruth;
+ std::vector<MatrixXd> points_groundtruth;
std::vector<PolylineRectangularClass> classification_groundtruth;
std::vector<PolylineRectangularClass> classification_candidates;
@@ -22,30 +22,30 @@ class LandmarkPolylineTest : public testing::Test
classification_candidates.push_back(PolylineClassPallet());
classification_candidates.push_back(PolylineRectangularClass(OTHER,45.6, 3.1));
- //std::vector<Scalar> object_L({12, 5.9, 1.2});
- //std::vector<Scalar> object_W({2.345, 2.345, 0.9});
+ //std::vector<double> object_L({12, 5.9, 1.2});
+ //std::vector<double> object_W({2.345, 2.345, 0.9});
//std::vector<PolylineClassType> object_class({CONTAINER, SMALL_CONTAINER, PALLET});
- StateBlockPtr lmk_p_ptr = std::make_shared<StateBlock>(Vector2s::Zero());
- StateBlockPtr lmk_o_ptr = std::make_shared<StateBlock>(Vector1s::Zero());
+ StateBlockPtr lmk_p_ptr = std::make_shared<StateBlock>(Vector2d::Zero());
+ StateBlockPtr lmk_o_ptr = std::make_shared<StateBlock>(Vector1d::Zero());
- MatrixXs points = MatrixXs::Zero(2,5);
+ MatrixXd points = MatrixXd::Zero(2,5);
points << 1, 1,-1,-1, 1,
2,-2,-2, 2, 2;
lmk_ptr = std::make_shared<LandmarkPolyline2D>(lmk_p_ptr, lmk_o_ptr, points, true, true);
// classification test
- MatrixXs class_points = MatrixXs::Zero(2,4);
+ MatrixXd class_points = MatrixXd::Zero(2,4);
for (auto configuration = 1; configuration <= 2; configuration++)
for (auto candidate_class : classification_candidates)
//for (unsigned int classification = 0; classification < object_L.size(); classification++)
{
- const Scalar& L = candidate_class.L;
- const Scalar& W = candidate_class.W;
- //const Scalar& L = object_L[classification];
- //const Scalar& W = object_W[classification];
+ const double& L = candidate_class.L;
+ const double& W = candidate_class.W;
+ //const double& L = object_L[classification];
+ //const double& W = object_W[classification];
// create trivial points in corresponding configuration
if (configuration == 1)
@@ -57,10 +57,10 @@ class LandmarkPolylineTest : public testing::Test
// rotate and translate "randomly"
- Vector3s center_pose; center_pose << 4, -9, M_PI/5;
- Matrix2s R = Rotation2D<Scalar>(center_pose(2)).matrix();
+ Vector3d center_pose; center_pose << 4, -9, M_PI/5;
+ Matrix2d R = Rotation2D<double>(center_pose(2)).matrix();
class_points = R * class_points;
- class_points += center_pose.head<2>() * VectorXs::Ones(4).transpose();
+ class_points += center_pose.head<2>() * VectorXd::Ones(4).transpose();
// store object and points groundtruth
lmks_groundtruth.push_back(center_pose);
@@ -126,8 +126,8 @@ TEST_F(LandmarkPolylineTest, Classify)
ASSERT_TRUE(st_pair.second->isFixed());
// test points are in the same place (compoping with object pose)
- Matrix2s R = Rotation2D<Scalar>(lmk_ptr->getO()->getState()(0)).matrix();
- VectorXs t = lmk_ptr->getP()->getState();
+ Matrix2d R = Rotation2D<double>(lmk_ptr->getO()->getState()(0)).matrix();
+ VectorXd t = lmk_ptr->getP()->getState();
for (unsigned int point_i = 0; point_i < 4; point_i++)
ASSERT_MATRIX_APPROX(R*lmk_ptr->getPointVector(point_i)+t, points_groundtruth[i].col(point_i), 1e-9);
}
diff --git a/test/gtest_polyline_2D.cpp b/test/gtest_polyline_2D.cpp
index 9bea7514518904df1a115152c2b85a457f4471e4..03ab62b3df4ad44b57c3714a275c2d3737fce4ab 100644
--- a/test/gtest_polyline_2D.cpp
+++ b/test/gtest_polyline_2D.cpp
@@ -9,8 +9,8 @@ class Polyline2DTest : public testing::Test
{
public:
- Matrix3s T_map_sens1, T_map_sens2, T_map_polyline_center, T_sens1_sens2;
- MatrixXs points_map, points_sens1, points_sens2;
+ Matrix3d T_map_sens1, T_map_sens2, T_map_polyline_center, T_sens1_sens2;
+ MatrixXd points_map, points_sens1, points_sens2;
virtual void SetUp()
{
@@ -19,21 +19,21 @@ class Polyline2DTest : public testing::Test
void SetUpPoints(int n, bool first_defined=true, bool last_defined=true)
{
// points
- MatrixXs points = MatrixXs::Random(3,n);
- points.bottomRows(1) = MatrixXs::Ones(1,n);
- T_map_polyline_center = pose2T2D(10*Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
+ MatrixXd points = MatrixXd::Random(3,n);
+ points.bottomRows(1) = MatrixXd::Ones(1,n);
+ T_map_polyline_center = pose2T2D(10*Vector2d::Random(), (rand() % 2000*M_PI)*0.001);
points_map = T_map_polyline_center * points;
//std::cout << "points_map: \n" << points_map << std::endl;
// sens1
- T_map_sens1 = pose2T2D(5*Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
+ T_map_sens1 = pose2T2D(5*Vector2d::Random(), (rand() % 2000*M_PI)*0.001);
points_sens1 = T_map_sens1.inverse() * points_map;
// move first point along the segment from the first to the second
if (!first_defined)
{
- Scalar rand_scale = (rand() % 2000 + 1)*0.001;
- Eigen::Vector2s new_first_point = points_sens1.col(1).head(2) + (points_sens1.col(0).head(2) - points_sens1.col(1).head(2)) * rand_scale;
+ double rand_scale = (rand() % 2000 + 1)*0.001;
+ Eigen::Vector2d new_first_point = points_sens1.col(1).head(2) + (points_sens1.col(0).head(2) - points_sens1.col(1).head(2)) * rand_scale;
std::cout << "old first point: " << points_sens1.col(0).head(2).transpose() << std::endl;
std::cout << "new first point: " << new_first_point.transpose() << std::endl;
@@ -49,7 +49,7 @@ class Polyline2DTest : public testing::Test
//std::cout << "points_sens1: \n" << points_sens1 << std::endl;
// sens2
- T_map_sens2 = pose2T2D(5*Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
+ T_map_sens2 = pose2T2D(5*Vector2d::Random(), (rand() % 2000*M_PI)*0.001);
points_sens2 = T_map_sens2.inverse() * points_map;
T_sens1_sens2 = T_map_sens1.inverse() * T_map_sens2;
@@ -60,18 +60,18 @@ class Polyline2DTest : public testing::Test
TEST_F(Polyline2DTest, Transformations)
{
- ASSERT_MATRIX_APPROX(Matrix3s::Identity(), pose2T2D(Vector2s::Zero(),0), 1e-6);
- ASSERT_MATRIX_APPROX(Matrix3s::Identity(), pose2T2D(Vector3s::Zero()), 1e-6);
- ASSERT_MATRIX_APPROX(Vector3s::Zero(), T2pose2D(Matrix3s::Identity()), 1e-6);
+ ASSERT_MATRIX_APPROX(Matrix3d::Identity(), pose2T2D(Vector2d::Zero(),0), 1e-6);
+ ASSERT_MATRIX_APPROX(Matrix3d::Identity(), pose2T2D(Vector3d::Zero()), 1e-6);
+ ASSERT_MATRIX_APPROX(Vector3d::Zero(), T2pose2D(Matrix3d::Identity()), 1e-6);
- Vector3s v_rand, v_subs;
- v_rand << Vector2s::Random(), pi2pi((rand() % 2000*M_PI)*0.001);
+ Vector3d v_rand, v_subs;
+ v_rand << Vector2d::Random(), pi2pi((rand() % 2000*M_PI)*0.001);
v_subs = v_rand - T2pose2D(pose2T2D(v_rand));
v_subs(2) = pi2pi(v_subs(2));
- ASSERT_MATRIX_APPROX(v_subs, Vector3s::Zero(), 1e-6);
+ ASSERT_MATRIX_APPROX(v_subs, Vector3d::Zero(), 1e-6);
- Matrix3s T_rand(Matrix3s::Identity());
- T_rand.topLeftCorner(2,2) = Rotation2D<Scalar>(v_rand(2)).matrix();
+ Matrix3d T_rand(Matrix3d::Identity());
+ T_rand.topLeftCorner(2,2) = Rotation2D<double>(v_rand(2)).matrix();
T_rand.topRightCorner(2,1) = v_rand.head(2);
ASSERT_MATRIX_APPROX(T_rand, pose2T2D(v_rand), 1e-6);
}
@@ -87,7 +87,7 @@ TEST_F(Polyline2DTest, computeRigidTrans8points)
SetUpPoints(8, first_defined, last_defined);
// compute rigid transformation
- Vector3s v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
+ Vector3d v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
ASSERT_MATRIX_APPROX(pose2T2D(v_sens1_sens2), T_sens1_sens2, 1e-6);
auto sq_error = computeSqDist(points_sens1,
@@ -105,7 +105,7 @@ TEST_F(Polyline2DTest, computeRigidTrans8points)
ASSERT_TRUE((sq_error < 1e-6).all());
// compute rigid transformation
- Vector3s v_sens1_map = computeRigidTrans(points_sens1, points_map);
+ Vector3d v_sens1_map = computeRigidTrans(points_sens1, points_map);
ASSERT_MATRIX_APPROX(pose2T2D(v_sens1_map), T_map_sens1.inverse(), 1e-6);
sq_error = computeSqDist(points_sens1,
@@ -135,7 +135,7 @@ TEST_F(Polyline2DTest, computeRigidTrans2points2defined)
SetUpPoints(2, first_defined, last_defined);
// compute rigid transformation
- Vector3s v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
+ Vector3d v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
ASSERT_MATRIX_APPROX(pose2T2D(v_sens1_sens2), T_sens1_sens2, 1e-6);
auto sq_error = computeSqDist(points_sens1,
@@ -153,7 +153,7 @@ TEST_F(Polyline2DTest, computeRigidTrans2points2defined)
ASSERT_TRUE((sq_error < 1e-6).all());
// compute rigid transformation
- Vector3s v_sens1_map = computeRigidTrans(points_sens1, points_map);
+ Vector3d v_sens1_map = computeRigidTrans(points_sens1, points_map);
ASSERT_MATRIX_APPROX(pose2T2D(v_sens1_map), T_map_sens1.inverse(), 1e-6);
sq_error = computeSqDist(points_sens1,
@@ -183,7 +183,7 @@ TEST_F(Polyline2DTest, computeRigidTrans3points2defined)
SetUpPoints(3, first_defined, last_defined);
// compute rigid transformation
- Vector3s v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
+ Vector3d v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
ASSERT_MATRIX_APPROX(pose2T2D(v_sens1_sens2), T_sens1_sens2, 1e-6);
auto sq_error = computeSqDist(points_sens1,
@@ -201,7 +201,7 @@ TEST_F(Polyline2DTest, computeRigidTrans3points2defined)
ASSERT_TRUE((sq_error < 1e-6).all());
// compute rigid transformation
- Vector3s v_sens1_map = computeRigidTrans(points_sens1, points_map, first_defined, last_defined);
+ Vector3d v_sens1_map = computeRigidTrans(points_sens1, points_map, first_defined, last_defined);
ASSERT_MATRIX_APPROX(pose2T2D(v_sens1_map), T_map_sens1.inverse(), 1e-6);
sq_error = computeSqDist(points_sens1,
@@ -231,7 +231,7 @@ TEST_F(Polyline2DTest, computeRigidTrans3points1defined)
SetUpPoints(3, first_defined, last_defined);
// compute rigid transformation
- Vector3s v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
+ Vector3d v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
ASSERT_MATRIX_APPROX(pose2T2D(v_sens1_sens2), T_sens1_sens2, 1e-6);
auto sq_error = computeSqDist(points_sens1,
@@ -249,7 +249,7 @@ TEST_F(Polyline2DTest, computeRigidTrans3points1defined)
ASSERT_TRUE((sq_error < 1e-6).all());
// compute rigid transformation
- Vector3s v_sens1_map = computeRigidTrans(points_sens1, points_map, first_defined, last_defined);
+ Vector3d v_sens1_map = computeRigidTrans(points_sens1, points_map, first_defined, last_defined);
ASSERT_MATRIX_APPROX(pose2T2D(v_sens1_map), T_map_sens1.inverse(), 1e-6);
sq_error = computeSqDist(points_sens1,
@@ -279,7 +279,7 @@ TEST_F(Polyline2DTest, computeRigidTrans2points1defined)
SetUpPoints(2, first_defined, last_defined);
// compute rigid transformation
- Vector3s v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
+ Vector3d v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
ASSERT_MATRIX_APPROX(pose2T2D(v_sens1_sens2), T_sens1_sens2, 1e-6);
auto sq_error = computeSqDist(points_sens1,
@@ -297,7 +297,7 @@ TEST_F(Polyline2DTest, computeRigidTrans2points1defined)
ASSERT_TRUE((sq_error < 1e-6).all());
// compute rigid transformation
- Vector3s v_sens1_map = computeRigidTrans(points_sens1, points_map, first_defined, last_defined);
+ Vector3d v_sens1_map = computeRigidTrans(points_sens1, points_map, first_defined, last_defined);
ASSERT_MATRIX_APPROX(pose2T2D(v_sens1_map), T_map_sens1.inverse(), 1e-6);
sq_error = computeSqDist(points_sens1,
@@ -327,7 +327,7 @@ TEST_F(Polyline2DTest, computeRigidTrans2points1definedBack)
SetUpPoints(2, first_defined, last_defined);
// compute rigid transformation
- Vector3s v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
+ Vector3d v_sens1_sens2 = computeRigidTrans(points_sens1, points_sens2, first_defined, last_defined);
ASSERT_MATRIX_APPROX(pose2T2D(v_sens1_sens2), T_sens1_sens2, 1e-6);
auto sq_error = computeSqDist(points_sens1,
@@ -345,7 +345,7 @@ TEST_F(Polyline2DTest, computeRigidTrans2points1definedBack)
ASSERT_TRUE((sq_error < 1e-6).all());
// compute rigid transformation
- Vector3s v_sens1_map = computeRigidTrans(points_sens1, points_map, first_defined, last_defined);
+ Vector3d v_sens1_map = computeRigidTrans(points_sens1, points_map, first_defined, last_defined);
ASSERT_MATRIX_APPROX(pose2T2D(v_sens1_map), T_map_sens1.inverse(), 1e-6);
sq_error = computeSqDist(points_sens1,
diff --git a/test/gtest_processor_odom_icp.cpp b/test/gtest_processor_odom_icp.cpp
index 96393abda2ab06bf7541b3710058a33e1d239f59..1e92c29c53f878f7a5d2bc0b11a923861ee8f42d 100644
--- a/test/gtest_processor_odom_icp.cpp
+++ b/test/gtest_processor_odom_icp.cpp
@@ -44,15 +44,20 @@ class ProcessorOdomICP_Test : public testing::Test
std::vector<float> ranges;
TimeStamp t0;
- Vector3s x0;
- Matrix3s P0;
+ Vector3d x0;
+ Matrix3d P0;
FrameBasePtr F0, F; // keyframes
virtual void SetUp()
{
problem = Problem::create("PO", 2);
solver = std::make_shared<CeresManager>(problem);
- auto sen = problem->installSensor("SensorLaser2D", "lidar", Eigen::Vector3s(0,0,0), laser_root_dir + "/test/yaml/sensor_laser_2D.yaml");
+//<<<<<<< HEAD
+ auto sen = problem->installSensor("SensorLaser2D", "lidar", Eigen::Vector3d(0,0,0), laser_root_dir + "/test/yaml/sensor_laser_2D.yaml");
+//=======
+//
+// auto sen = problem->installSensor("LASER 2D", "lidar", Eigen::Vector3d(0,0,0), laser_root_dir + "/test/yaml/sensor_laser_2D.yaml");
+//>>>>>>> 8-replace-scalar-to-double
lidar = std::static_pointer_cast<SensorLaser2D>(sen);
auto prc = problem->installProcessor("ProcessorOdomIcp", "prc icp", "lidar", laser_root_dir + "/test/yaml/processor_odom_ICP.yaml");
@@ -61,7 +66,7 @@ class ProcessorOdomICP_Test : public testing::Test
ranges = std::vector<float>({49.97591781616211, 49.996429443359375, 49.999759674072266, 50.0, 50.0, 50.0, 49.998634338378906, 50.0, 50.0, 49.99236297607422, 49.99384307861328, 50.0, 49.9869270324707, 50.0, 49.99005889892578, 49.99773406982422, 50.0, 49.98741912841797, 50.0, 49.99842071533203, 50.0, 49.99243927001953, 49.997291564941406, 50.0, 50.0, 49.98580551147461, 49.991844177246094, 49.98896026611328, 50.0, 50.0, 49.9897346496582, 49.998111724853516, 49.99882125854492, 50.0, 50.0, 50.0, 50.0, 49.999698638916016, 50.0, 50.0, 50.0, 50.0, 49.991397857666016, 49.99360275268555, 49.999027252197266, 49.99750900268555, 49.99100112915039, 49.998714447021484, 49.98794174194336, 50.0, 50.0, 50.0, 50.0, 50.0, 49.98186492919922, 50.0, 50.0, 50.0, 49.99155807495117, 49.997196197509766, 49.98872375488281, 49.99138259887695, 50.0, 49.99021530151367, 49.99164581298828, 50.0, 49.991390228271484, 50.0, 50.0, 50.0, 49.997249603271484, 50.0, 49.991851806640625, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 49.97983169555664, 49.98630142211914, 50.0, 20.6888370513916, 49.9797477722168, 49.98846435546875, 49.99418640136719, 50.0, 50.0, 50.0, 49.99342346191406, 50.0, 49.9906005859375, 50.0, 49.99853515625, 49.989444732666016, 38.552852630615234, 38.28703689575195, 38.04865264892578, 37.78112030029297, 37.54747772216797, 37.28171157836914, 37.206565856933594, 36.835411071777344, 36.61864471435547, 36.39655685424805, 36.1579475402832, 35.95964431762695, 35.75605773925781, 35.552188873291016, 35.75926208496094, 36.27781677246094, 35.16993713378906, 34.99699401855469, 34.82910919189453, 34.6483039855957, 34.48637390136719, 34.32539749145508, 34.16202163696289, 34.232051849365234, 33.86043167114258, 33.71691131591797, 33.566650390625, 33.42384338378906, 33.2882080078125, 33.16693115234375, 33.041419982910156, 32.906009674072266, 33.008323669433594, 33.706356048583984, 34.43825149536133, 35.25088119506836, 36.05652618408203, 36.930118560791016, 37.83384704589844, 33.12321472167969, 33.02351760864258, 32.9192008972168, 32.82925796508789, 32.74382781982422, 32.64959716796875, 32.580204010009766, 32.49120330810547, 33.05714797973633, 32.343536376953125, 32.26381301879883, 32.21063232421875, 32.12488555908203, 32.06965255737305, 32.0222282409668, 31.954957962036133, 31.903532028198242, 31.83578872680664, 32.51456832885742, 34.189456939697266, 31.12668228149414, 31.076339721679688, 31.047151565551758, 30.967018127441406, 30.956220626831055, 30.924589157104492, 30.893285751342773, 30.869199752807617, 30.843050003051758, 32.791847229003906, 30.809431076049805, 30.79128074645996, 30.779237747192383, 30.776460647583008, 30.74305534362793, 30.74994468688965, 30.7137393951416, 30.734609603881836, 30.719928741455078, 30.71673011779785, 49.99970626831055, 50.0, 49.987911224365234, 33.68583679199219, 31.76846694946289, 31.8026123046875, 31.802202224731445, 31.818490982055664, 31.85223960876465, 31.86141014099121, 31.906801223754883, 31.93423843383789, 31.964210510253906, 33.567230224609375, 32.055015563964844, 32.07001876831055, 32.13076400756836, 32.16000747680664, 32.22781753540039, 32.26890563964844, 32.323944091796875, 32.36326217651367, 32.430908203125, 49.980655670166016, 34.32135772705078, 33.09465789794922, 32.27247619628906, 32.33710479736328, 32.41763687133789, 32.498661041259766, 32.57213592529297, 32.67158126831055, 32.74591827392578, 32.814476013183594, 32.93477249145508, 33.04751968383789, 33.136863708496094, 33.23999786376953, 33.34675979614258, 33.42970657348633, 33.53573226928711, 33.66716003417969, 33.78378677368164, 33.905670166015625, 34.02836608886719, 34.151817321777344, 34.2794189453125, 34.41516876220703, 34.551273345947266, 34.702728271484375, 34.84151840209961, 34.986881256103516, 35.162757873535156, 35.332794189453125, 35.47941970825195, 35.65633010864258, 35.82624435424805, 36.00060272216797, 36.17729187011719, 36.36515808105469, 36.55763626098633, 36.744773864746094, 38.46407699584961, 37.869808197021484, 37.767921447753906, 37.958900451660156, 38.20857620239258, 38.38622283935547, 38.68323516845703, 38.871334075927734, 39.151519775390625, 39.377750396728516, 39.68268966674805, 39.89873123168945, 40.197330474853516, 40.47549819946289, 40.73743438720703, 41.04566955566406, 42.33650207519531, 41.92591094970703, 41.43912124633789, 41.045528411865234, 41.32114028930664, 41.581878662109375, 41.944580078125, 42.318912506103516, 42.6725959777832, 43.07264709472656, 43.443477630615234, 43.83216094970703, 44.19996643066406, 44.63225555419922, 45.06049346923828, 45.468536376953125, 45.89896774291992, 46.330604553222656, 46.778343200683594, 47.31666946411133, 47.789310455322266, 48.26376724243164, 48.826602935791016, 49.33188247680664, 49.990909576416016, 50.0, 50.0, 50.0, 50.0, 49.995697021484375, 49.99568176269531, 49.98163986206055, 50.0, 50.0, 49.9764289855957, 50.0, 50.0, 49.98639678955078, 49.99431228637695, 50.0, 50.0, 50.0, 50.0, 49.9874267578125, 50.0, 49.98714828491211, 50.0, 49.99470901489258, 49.99113464355469, 50.0, 50.0, 50.0, 49.985504150390625, 49.99067306518555, 50.0, 49.997161865234375, 50.0, 50.0, 50.0, 49.995513916015625, 49.993038177490234, 50.0, 49.99763107299805, 50.0, 49.98752975463867, 50.0, 49.988037109375, 50.0, 50.0, 50.0, 49.9975700378418, 50.0, 49.998756408691406, 49.97819519042969, 49.99104690551758, 49.99087905883789, 49.94268798828125, 49.85968017578125, 49.786617279052734, 49.70594787597656, 49.62562561035156, 49.56686782836914, 49.50475311279297, 49.416934967041016, 49.35699462890625, 49.308589935302734, 49.990482330322266, 50.0, 49.998836517333984, 50.0, 50.0, 50.0, 50.0, 50.0, 50.0, 49.980472564697266, 49.99903869628906, 50.0, 50.0, 49.989845275878906, 49.98395919799805, 50.0, 49.99302673339844, 49.99530792236328, 49.99745559692383, 50.0, 49.99560546875, 21.569303512573242});
t0 = 0.0;
- x0 = Vector3s(0,0,0);
+ x0 = Vector3d(0,0,0);
P0 . setIdentity();
F0 = problem->setPrior(x0, P0, t0, 0.1);
}
@@ -133,7 +138,7 @@ TEST_F(ProcessorOdomICP_Test, full)
if (i >= 2 && i <= 4)
{
// perturb F1
- F->setState(0.1 * Vector3s::Random());
+ F->setState(0.1 * Vector3d::Random());
WOLF_TRACE("F", F->id() , " ", F->getState().transpose(), " perturbed!");
}
@@ -160,7 +165,7 @@ TEST_F(ProcessorOdomICP_Test, solve)
for (auto F : problem->getTrajectory()->getFrameList())
{
if (F->isKey())
- F->setState(0.5 * Vector3s::Random());
+ F->setState(0.5 * Vector3d::Random());
}
std::string report = solver->solve(SolverManager::ReportVerbosity::BRIEF);
diff --git a/test/gtest_processor_tracker_feature_polyline_2D.cpp b/test/gtest_processor_tracker_feature_polyline_2D.cpp
index 51d6b6713d0710bd77e47377e9b9332061f8b6b8..8324f69dd9a25eeb4ae4140d66581060b13addeb 100644
--- a/test/gtest_processor_tracker_feature_polyline_2D.cpp
+++ b/test/gtest_processor_tracker_feature_polyline_2D.cpp
@@ -102,7 +102,7 @@ class ProcessorPolylinePublicMethods : public ProcessorTrackerFeaturePolyline2D
void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches,
const FeaturePolyline2DPtr& _ftr_last,
const FeaturePolyline2DPtr& _ftr_incoming,
- const Eigen::Matrix3s& _T_last_incoming_prior) const
+ const Eigen::Matrix3d& _T_last_incoming_prior) const
{
ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature( ftr_matches,
_ftr_last,
@@ -113,7 +113,7 @@ class ProcessorPolylinePublicMethods : public ProcessorTrackerFeaturePolyline2D
void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches,
const LandmarkPolyline2DPtr& _lmk_ptr,
const FeaturePolyline2DPtr& _feat_ptr,
- const Eigen::Matrix3s& _T_feature_landmark_prior) const
+ const Eigen::Matrix3d& _T_feature_landmark_prior) const
{
ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(lmk_matches,
_lmk_ptr,
@@ -147,11 +147,11 @@ class ProcessorPolyline2Dt : public testing::Test
ProcessorPolylinePublicMethodsPtr processor_pl;
wolf::SensorBasePtr sensor_ptr;
wolf::TimeStamp t;
- wolf::Scalar mti_clock, tmp;
- wolf::Scalar dt;
- Vector6s data;
- Matrix6s data_cov;
- VectorXs x0;
+ double mti_clock, tmp;
+ double dt;
+ Vector6d data;
+ Matrix6d data_cov;
+ VectorXd x0;
std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr;
//a new of this will be created for each new test
@@ -168,19 +168,19 @@ class ProcessorPolyline2Dt : public testing::Test
// Wolf problem
problem = Problem::create("POV", 3);
- Vector7s extrinsics = (Vector7s() << 0,0,0, 0,0,0,1).finished();
+ Vector7d extrinsics = (Vector7d() << 0,0,0, 0,0,0,1).finished();
sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml");
// Time and data variables
- data = Vector6s::Zero();
- data_cov = Matrix6s::Identity();
+ data = Vector6d::Zero();
+ data_cov = Matrix6d::Identity();
// Set the origin
x0.resize(10);
// Create one capture to store the IMU data arriving from (sensor / callback / file / etc.)
- cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
+ cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
}
virtual void TearDown()
@@ -217,7 +217,7 @@ TEST(ProcessorIMU_constructors, ALL)
//Factory constructor without yaml
std::string wolf_root = _WOLF_ROOT_DIR;
ProblemPtr problem = Problem::create("POV", 3);
- Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
+ Vector7d extrinsics = (Vector7d()<<1,0,0, 0,0,0,1).finished();
SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", "");
ProcessorParamsIMU params_default;
@@ -247,7 +247,7 @@ TEST(ProcessorIMU, voteForKeyFrame)
// Wolf problem
ProblemPtr problem = Problem::create("POV", 3);
- Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished();
+ Vector7d extrinsics = (Vector7d()<<1,0,0, 0,0,0,1).finished();
SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml");
ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>();
prc_imu_params->max_time_span = 1;
@@ -258,24 +258,24 @@ TEST(ProcessorIMU, voteForKeyFrame)
ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params);
//setting origin
- VectorXs x0(10);
+ VectorXd x0(10);
TimeStamp t(0);
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
//data variable and covariance matrix
// since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions
- Vector6s data;
+ Vector6d data;
data << 1,0,0, 0,0,0;
- Matrix6s data_cov(Matrix6s::Identity());
+ Matrix6d data_cov(Matrix6d::Identity());
data_cov(0,0) = 0.5;
// Create the captureIMU to store the IMU data arriving from (sensor / callback / file / etc.)
- std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero());
+ std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6d::Zero());
// Time
// we want more than one data to integrate otherwise covariance will be 0
- Scalar dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() - 0.1;
+ double dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() - 0.1;
t.set(dt);
cap_imu_ptr->setTimeStamp(t);
sensor_ptr->process(cap_imu_ptr);
@@ -324,7 +324,7 @@ TEST_F(ProcessorIMUt, interpolate)
t.set(0);
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
data << 2, 0, 0, 0, 0, 0; // only acc_x
@@ -370,7 +370,7 @@ TEST_F(ProcessorIMUt, acc_x)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
@@ -380,9 +380,9 @@ TEST_F(ProcessorIMUt, acc_x)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- VectorXs x(10);
+ VectorXd x(10);
x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2
- Vector6s b; b << 0,0,0, 0,0,0;
+ Vector6d b; b << 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
@@ -396,7 +396,7 @@ TEST_F(ProcessorIMUt, acc_y)
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
data << 0, 20, 9.806, 0, 0, 0; // only acc_y, but measure gravity!
@@ -406,9 +406,9 @@ TEST_F(ProcessorIMUt, acc_y)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- VectorXs x(10);
+ VectorXd x(10);
x << 0,0.00001,0, 0,0,0,1, 0,0.02,0; // advanced at a=20m/s2 during 0.001s ==> dx = 0.5*20*0.001^2 = 0.00001; dvx = 20*0.001 = 0.02
- Vector6s b; b<< 0,0,0, 0,0,0;
+ Vector6d b; b<< 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
@@ -433,7 +433,7 @@ TEST_F(ProcessorIMUt, acc_z)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
data << 0, 0, 9.806 + 2.0, 0, 0, 0; // only acc_z, but measure gravity!
@@ -443,9 +443,9 @@ TEST_F(ProcessorIMUt, acc_z)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- VectorXs x(10);
+ VectorXd x(10);
x << 0,0,0.01, 0,0,0,1, 0,0,0.2; // advanced at a=2m/s2 during 0.1s ==> dz = 0.5*2*0.1^2 = 0.01; dvz = 2*0.1 = 0.2
- Vector6s b; b<< 0,0,0, 0,0,0;
+ Vector6d b; b<< 0,0,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL);
@@ -470,7 +470,7 @@ TEST_F(ProcessorIMUt, check_Covariance)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity!
@@ -479,22 +479,22 @@ TEST_F(ProcessorIMUt, check_Covariance)
cap_imu_ptr->setTimeStamp(0.1);
sensor_ptr->process(cap_imu_ptr);
- VectorXs delta_preint(problem->getProcessorMotionPtr()->getMotion().delta_integr_);
-// Matrix<wolf::Scalar,9,9> delta_preint_cov = problem->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
+ VectorXd delta_preint(problem->getProcessorMotionPtr()->getMotion().delta_integr_);
+// Matrix<double,9,9> delta_preint_cov = problem->getProcessorMotionPtr()->getCurrentDeltaPreintCov();
ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL));
-// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXs::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
+// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXd::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation
}
TEST_F(ProcessorIMUt, gyro_x)
{
- wolf::Scalar dt(0.001);
+ double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -502,10 +502,10 @@ TEST_F(ProcessorIMUt, gyro_x)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd x(10);
x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
@@ -517,7 +517,7 @@ TEST_F(ProcessorIMUt, gyro_x)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity());
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -532,16 +532,16 @@ TEST_F(ProcessorIMUt, gyro_x)
TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
{
// time
- wolf::Scalar dt(0.001);
+ double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
// bias
- wolf::Scalar abx = 0.002;
- Vector6s bias; bias << abx,0,0, 0,0,0;
- Vector3s acc_bias = bias.head(3);
+ double abx = 0.002;
+ Vector6d bias; bias << abx,0,0, 0,0,0;
+ Vector3d acc_bias = bias.head(3);
// state
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
// init things
problem->setPrior(x0, P0, t, 0.01);
@@ -551,7 +551,7 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose());
// data
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << acc_bias - wolf::gravity(), rate_of_turn, 0, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -559,13 +559,13 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x_true(10);
+ VectorXd x_true(10);
x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
- VectorXs x_est(10);
+ VectorXd x_est(10);
x_est = problem->getCurrentState().head(10);
ASSERT_MATRIX_APPROX(x_est.transpose() , x_true.transpose(), wolf::Constants::EPS);
@@ -577,7 +577,7 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -592,20 +592,20 @@ TEST_F(ProcessorIMUt, gyro_x_biasedAbx)
TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
{
- wolf::Scalar dt(0.001);
+ double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
- wolf::Scalar abx(0.002), aby(0.01);
+ double abx(0.002), aby(0.01);
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- Vector6s bias; bias << abx,aby,0, 0,0,0;
- Vector3s acc_bias = bias.head(3);
+ Vector6d bias; bias << abx,aby,0, 0,0,0;
+ Vector3d acc_bias = bias.head(3);
problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias);
problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
// data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity
data << acc_bias - wolf::gravity(), rate_of_turn*1.5, 0, 0; // measure gravity
@@ -614,10 +614,10 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd x(10);
x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;//, abx,aby,0, 0,0,0;
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);// << "expected state : " << problem->getCurrentState().transpose()
@@ -630,7 +630,7 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias;
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -645,13 +645,13 @@ TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy)
TEST_F(ProcessorIMUt, gyro_z)
{
- wolf::Scalar dt(0.001);
+ double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
cap_imu_ptr->setData(data);
@@ -659,10 +659,10 @@ TEST_F(ProcessorIMUt, gyro_z)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd x(10);
x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
@@ -686,18 +686,18 @@ TEST_F(ProcessorIMUt, gyro_xyz)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- Vector3s tmp_vec; //will be used to store rate of turn data
- Quaternions quat_comp(Quaternions::Identity());
- Matrix3s R0(Matrix3s::Identity());
- wolf::Scalar time = 0;
+ Vector3d tmp_vec; //will be used to store rate of turn data
+ Quaterniond quat_comp(Quaterniond::Identity());
+ Matrix3d R0(Matrix3d::Identity());
+ double time = 0;
const unsigned int x_rot_vel = 5;
const unsigned int y_rot_vel = 6;
const unsigned int z_rot_vel = 13;
- wolf::Scalar tmpx, tmpy, tmpz;
+ double tmpx, tmpy, tmpz;
/*
ox oy oz evolution in degrees (for understanding) --> converted in rad
@@ -713,7 +713,7 @@ TEST_F(ProcessorIMUt, gyro_xyz)
wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
*/
- const wolf::Scalar dt = 0.001;
+ const double dt = 0.001;
for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++)
{
@@ -728,7 +728,7 @@ TEST_F(ProcessorIMUt, gyro_xyz)
quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
R0 = R0 * wolf::v2R(tmp_vec*dt);
// use processorIMU
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
data.tail(3) = tmp_vec;
@@ -746,16 +746,16 @@ TEST_F(ProcessorIMUt, gyro_xyz)
*/
// validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
- Quaternions R2quat(wolf::v2q(wolf::R2v(R0)));
- Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
- Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
+ Quaterniond R2quat(wolf::v2q(wolf::R2v(R0)));
+ Vector4d quat_comp_vec((Vector4d() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
+ Vector4d R2quat_vec((Vector4d() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS);// << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
- VectorXs x(10);
+ VectorXd x(10);
x << 0,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0,0,0;
- Quaternions result_quat(problem->getCurrentState().data() + 3);
+ Quaterniond result_quat(problem->getCurrentState().data() + 3);
//std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
//check position part
@@ -770,13 +770,13 @@ TEST_F(ProcessorIMUt, gyro_xyz)
TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
{
- wolf::Scalar dt(0.001);
+ double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity!
cap_imu_ptr->setData(data);
@@ -784,10 +784,10 @@ TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd x(10);
x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002m
ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);
@@ -809,13 +809,13 @@ TEST_F(ProcessorIMUt, gyro_z_ConstVelocity)
TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
{
- wolf::Scalar dt(0.001);
+ double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -823,10 +823,10 @@ TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
@@ -839,7 +839,7 @@ TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity());
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -853,13 +853,13 @@ TEST_F(ProcessorIMUt, gyro_x_ConstVelocity)
TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
{
- wolf::Scalar dt(0.001);
+ double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -867,10 +867,10 @@ TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
@@ -883,7 +883,7 @@ TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity());
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -897,13 +897,13 @@ TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity)
TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
{
- wolf::Scalar dt(0.001);
+ double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -911,10 +911,10 @@ TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002
x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0;
@@ -927,7 +927,7 @@ TEST_F(ProcessorIMUt, gyro_y_ConstVelocity)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity());
cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000
@@ -943,18 +943,18 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
{
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 2,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- Vector3s tmp_vec; //will be used to store rate of turn data
- Quaternions quat_comp(Quaternions::Identity());
- Matrix3s R0(Matrix3s::Identity());
- wolf::Scalar time = 0;
+ Vector3d tmp_vec; //will be used to store rate of turn data
+ Quaterniond quat_comp(Quaterniond::Identity());
+ Matrix3d R0(Matrix3d::Identity());
+ double time = 0;
const unsigned int x_rot_vel = 5;
const unsigned int y_rot_vel = 6;
const unsigned int z_rot_vel = 13;
- wolf::Scalar tmpx, tmpy, tmpz;
+ double tmpx, tmpy, tmpz;
/*
ox oy oz evolution in degrees (for understanding) --> converted in rad
@@ -970,7 +970,7 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
wz = pi*gamma*cos(gamma*t*pi/180)*pi/180;
*/
- const wolf::Scalar dt = 0.001;
+ const double dt = 0.001;
for(unsigned int data_iter = 0; data_iter < 1000; data_iter ++)
{
@@ -985,7 +985,7 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
quat_comp = quat_comp * wolf::v2q(tmp_vec*dt);
R0 = R0 * wolf::v2R(tmp_vec*dt);
// use processorIMU
- Quaternions rot(problem->getCurrentState().data()+3);
+ Quaterniond rot(problem->getCurrentState().data()+3);
data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured
data.tail(3) = tmp_vec;
@@ -1003,17 +1003,17 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
*/
// validating that the quaternion composition and rotation matrix composition actually describe the same rotation.
- Quaternions R2quat(wolf::v2q(wolf::R2v(R0)));
- Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
- Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
+ Quaterniond R2quat(wolf::v2q(wolf::R2v(R0)));
+ Vector4d quat_comp_vec((Vector4d() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() );
+ Vector4d R2quat_vec((Vector4d() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() );
ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS); // << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl;
- VectorXs x(10);
+ VectorXd x(10);
//rotation + translation due to initial velocity
x << 2,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 2,0,0;
- Quaternions result_quat(problem->getCurrentState().data() + 3);
+ Quaterniond result_quat(problem->getCurrentState().data() + 3);
//std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl;
//check position part
@@ -1029,13 +1029,13 @@ TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity)
TEST_F(ProcessorIMUt, gyro_x_acc_x)
{
- wolf::Scalar dt(0.001);
+ double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << 1, 0, -wolf::gravity()(2), rate_of_turn, 0, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -1043,10 +1043,10 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on x axis
// translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on x axis
// v = a*dt = 0.001
@@ -1062,8 +1062,8 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<1,0,0).finished();
+ Quaterniond rot(problem->getCurrentState().data()+3);
+ data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3d()<<1,0,0).finished();
cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
cap_imu_ptr->setData(data);
@@ -1078,13 +1078,13 @@ TEST_F(ProcessorIMUt, gyro_x_acc_x)
TEST_F(ProcessorIMUt, gyro_y_acc_y)
{
- wolf::Scalar dt(0.001);
+ double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << 0, 1, -wolf::gravity()(2), 0, rate_of_turn, 0; // measure gravity
cap_imu_ptr->setData(data);
@@ -1092,10 +1092,10 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on y axis
// translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on y axis
// v = a*dt = 0.001
@@ -1111,8 +1111,8 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,1,0).finished();
+ Quaterniond rot(problem->getCurrentState().data()+3);
+ data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3d()<<0,1,0).finished();
cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
cap_imu_ptr->setData(data);
@@ -1127,13 +1127,13 @@ TEST_F(ProcessorIMUt, gyro_y_acc_y)
TEST_F(ProcessorIMUt, gyro_z_acc_z)
{
- wolf::Scalar dt(0.001);
+ double dt(0.001);
t.set(0); // clock in 0,1 ms ticks
x0 << 0,0,0, 0,0,0,1, 0,0,0;
- MatrixXs P0(9,9); P0.setIdentity();
+ MatrixXd P0(9,9); P0.setIdentity();
problem->setPrior(x0, P0, t, 0.01);
- wolf::Scalar rate_of_turn = 5 * M_PI/180.0;
+ double rate_of_turn = 5 * M_PI/180.0;
data << 0, 0, -wolf::gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity
cap_imu_ptr->setData(data);
@@ -1141,10 +1141,10 @@ TEST_F(ProcessorIMUt, gyro_z_acc_z)
sensor_ptr->process(cap_imu_ptr);
// Expected state after one integration
- Quaternions quat_comp(Quaternions::Identity());
+ Quaterniond quat_comp(Quaterniond::Identity());
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- VectorXs x(10);
+ VectorXd x(10);
// rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on z axis
// translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on z axis
// v = a*dt = 0.001
@@ -1159,8 +1159,8 @@ TEST_F(ProcessorIMUt, gyro_z_acc_z)
// quaternion composition
quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt);
- Quaternions rot(problem->getCurrentState().data()+3);
- data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,0,1).finished();
+ Quaterniond rot(problem->getCurrentState().data()+3);
+ data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3d()<<0,0,1).finished();
cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000
cap_imu_ptr->setData(data);