Skip to content
Snippets Groups Projects

WIP: Resolve "Remove wolf::Scalar and use double instead"

Merged Joan Solà Ortega requested to merge 264-remove-wolf-scalar-and-use-double-instead into devel
Files
166
@@ -116,10 +116,10 @@ int main(int argc, char *argv[])
// INITIALIZATION ============================================================================================
//init random generators
Scalar odom_std_factor = 0.1;
Scalar gps_std = 10;
double odom_std_factor = 0.1;
double gps_std = 10;
std::default_random_engine generator(1);
std::normal_distribution<Scalar> gaussian_distribution(0.0, 1);
std::normal_distribution<double> gaussian_distribution(0.0, 1);
// Faramotics stuff
Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
@@ -147,18 +147,18 @@ int main(int argc, char *argv[])
myScanner->loadAssimpModel(modelFileName);
//variables
Eigen::Vector3s odom_reading;
Eigen::Vector2s gps_fix_reading;
Eigen::VectorXs pose_odom(3); //current odometry integred pose
Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
Eigen::Vector3d odom_reading;
Eigen::Vector2d gps_fix_reading;
Eigen::VectorXd pose_odom(3); //current odometry integred pose
Eigen::VectorXd ground_truth(n_execution * 3); //all true poses
Eigen::VectorXd odom_trajectory(n_execution * 3); //open loop trajectory
Eigen::VectorXd mean_times = Eigen::VectorXd::Zero(7);
clock_t t1, t2;
// Wolf manager initialization
Eigen::Vector3s odom_pose = Eigen::Vector3s::Zero();
Eigen::Vector3s gps_pose = Eigen::Vector3s::Zero();
Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
Eigen::Vector3d odom_pose = Eigen::Vector3d::Zero();
Eigen::Vector3d gps_pose = Eigen::Vector3d::Zero();
Eigen::Vector4d laser_1_pose, laser_2_pose; //xyz + theta
laser_1_pose << 1.2, 0, 0, 0; //laser 1
laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
SensorOdom2D odom_sensor(std::make_shared<StateBlock>(odom_pose.head(2)), std::make_shared<StateBlock>(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
@@ -171,8 +171,8 @@ int main(int argc, char *argv[])
ground_truth.head(3) = pose_odom;
odom_trajectory.head(3) = pose_odom;
WolfManager* wolf_manager_QR = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
WolfManager* wolf_manager_ceres = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01);
WolfManager* wolf_manager_QR = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution*10, 0.01);
WolfManager* wolf_manager_ceres = new WolfManager(FRM_PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3d::Identity() * 0.01, n_execution*10, 0.01);
// Ceres initialization
ceres::Solver::Options ceres_options;
@@ -242,10 +242,10 @@ int main(int argc, char *argv[])
// ADD CAPTURES ---------------------------
//std::cout << "ADD CAPTURES..." << std::endl;
// adding new sensor captures
wolf_manager_QR->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
wolf_manager_QR->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXs::Identity(3,3)));
wolf_manager_ceres->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
wolf_manager_ceres->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXs::Identity(3,3)));
wolf_manager_QR->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXd::Identity(2,2)));
wolf_manager_QR->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3,3)));
wolf_manager_ceres->addCapture(new CaptureOdom2D(TimeStamp(), TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXd::Identity(2,2)));
wolf_manager_ceres->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * gps_std * Eigen::MatrixXd::Identity(3,3)));
//wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
//wolf_manager->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
// updating problem
@@ -291,7 +291,7 @@ int main(int argc, char *argv[])
std::vector<double> landmark_vector;
for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
{
Scalar* position_ptr = (*landmark_it)->getP()->get();
double* position_ptr = (*landmark_it)->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -346,7 +346,7 @@ int main(int argc, char *argv[])
std::vector<double> landmark_vector;
for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
{
Scalar* position_ptr = (*landmark_it)->getP()->get();
double* position_ptr = (*landmark_it)->getP()->get();
landmark_vector.push_back(*position_ptr); //x
landmark_vector.push_back(*(position_ptr + 1)); //y
landmark_vector.push_back(0.2); //z
@@ -364,7 +364,7 @@ int main(int argc, char *argv[])
// Vehicle poses
std::cout << "Vehicle poses..." << std::endl;
int i = 0;
Eigen::VectorXs state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().size() * 3);
Eigen::VectorXd state_poses(wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().size() * 3);
for (auto frame_it = wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().begin(); frame_it != wolf_manager_QR->getProblem()->getTrajectory()->getFrameList().end(); frame_it++)
{
if (complex_angle)
@@ -377,10 +377,10 @@ int main(int argc, char *argv[])
// Landmarks
std::cout << "Landmarks..." << std::endl;
i = 0;
Eigen::VectorXs landmarks(wolf_manager_QR->getProblem()->getMap()->getLandmarkList().size() * 2);
Eigen::VectorXd landmarks(wolf_manager_QR->getProblem()->getMap()->getLandmarkList().size() * 2);
for (auto landmark_it = wolf_manager_QR->getProblem()->getMap()->getLandmarkList().begin(); landmark_it != wolf_manager_QR->getProblem()->getMap()->getLandmarkList().end(); landmark_it++)
{
Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getP()->get());
Eigen::Map<Eigen::Vector2d> landmark((*landmark_it)->getP()->get());
landmarks.segment(i, 2) = landmark;
i += 2;
}
Loading