diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt index f5c2600bd0917e63bb914707c5e182f5c6a15ce7..b6a6410f6d308f31feed72d0e7c97711456419a3 100644 --- a/src/examples/CMakeLists.txt +++ b/src/examples/CMakeLists.txt @@ -2,76 +2,39 @@ ADD_EXECUTABLE(test_node_linked test_node_linked.cpp) TARGET_LINK_LIBRARIES(test_node_linked ${PROJECT_NAME}) -# numeric test -ADD_EXECUTABLE(test_ceres_wrapper_numeric test_ceres_wrapper_numeric.cpp) -TARGET_LINK_LIBRARIES(test_ceres_wrapper_numeric ${PROJECT_NAME}) - -# jet test -ADD_EXECUTABLE(test_ceres_wrapper_jet test_ceres_wrapper_jet.cpp) -TARGET_LINK_LIBRARIES(test_ceres_wrapper_jet ${PROJECT_NAME}) - -# jet individual blocks test -ADD_EXECUTABLE(test_ceres_wrapper_jet_ind_block test_ceres_wrapper_jet_ind_block.cpp) -TARGET_LINK_LIBRARIES(test_ceres_wrapper_jet_ind_block ${PROJECT_NAME}) - -# jet test with 2 sensors -ADD_EXECUTABLE(test_ceres_wrapper_jet_2_sensors test_ceres_wrapper_jet_2_sensors.cpp) -TARGET_LINK_LIBRARIES(test_ceres_wrapper_jet_2_sensors ${PROJECT_NAME}) - -# jet test manager -#ADD_EXECUTABLE(test_ceres_manager test_ceres_manager.cpp) -#TARGET_LINK_LIBRARIES(test_ceres_manager ${PROJECT_NAME}) - -# Testing a full wolf tree avoiding template classes for NodeLinked derived classes -ADD_EXECUTABLE(test_ceres_odom_batch test_ceres_odom_batch.cpp) -TARGET_LINK_LIBRARIES(test_ceres_odom_batch ${PROJECT_NAME}) - -# Testing a full wolf tree avoiding template classes for NodeLinked derived classes -ADD_EXECUTABLE(test_ceres_odom_iterative test_ceres_odom_iterative.cpp) -TARGET_LINK_LIBRARIES(test_ceres_odom_iterative ${PROJECT_NAME}) - # Testing Eigen Permutations -ADD_EXECUTABLE(test_permutations test_permutations.cpp) +ADD_EXECUTABLE(test_permutations solver/test_permutations.cpp) TARGET_LINK_LIBRARIES(test_permutations ${PROJECT_NAME}) - IF(Suitesparse_FOUND) # Testing a ccolamd test - ADD_EXECUTABLE(test_ccolamd test_ccolamd.cpp) + ADD_EXECUTABLE(test_ccolamd solver/test_ccolamd.cpp) TARGET_LINK_LIBRARIES(test_ccolamd ${PROJECT_NAME}) # Testing a blocks ccolamd test - ADD_EXECUTABLE(test_ccolamd_blocks test_ccolamd_blocks.cpp) + ADD_EXECUTABLE(test_ccolamd_blocks solver/test_ccolamd_blocks.cpp) TARGET_LINK_LIBRARIES(test_ccolamd_blocks ${PROJECT_NAME}) # Testing an incremental blocks ccolamd test - ADD_EXECUTABLE(test_incremental_ccolamd_blocks test_incremental_ccolamd_blocks.cpp) + ADD_EXECUTABLE(test_incremental_ccolamd_blocks solver/test_incremental_ccolamd_blocks.cpp) TARGET_LINK_LIBRARIES(test_incremental_ccolamd_blocks ${PROJECT_NAME}) # Testing an incremental QR with block ccolamd test - ADD_EXECUTABLE(test_iQR test_iQR.cpp) + ADD_EXECUTABLE(test_iQR solver/test_iQR.cpp) TARGET_LINK_LIBRARIES(test_iQR ${PROJECT_NAME}) # Testing QR solver test tending to wolf - ADD_EXECUTABLE(test_iQR_wolf test_iQR_wolf.cpp) + ADD_EXECUTABLE(test_iQR_wolf solver/test_iQR_wolf.cpp) TARGET_LINK_LIBRARIES(test_iQR_wolf ${PROJECT_NAME}) # Testing SPQR simple test - #ADD_EXECUTABLE(test_SPQR test_SPQR.cpp) + #ADD_EXECUTABLE(test_SPQR solver/test_SPQR.cpp) #TARGET_LINK_LIBRARIES(test_SPQR ${PROJECT_NAME}) ENDIF(Suitesparse_FOUND) # Building and populating the wolf tree -# ADD_EXECUTABLE(test_wolf_tree test_wolf_tree.cpp) -# TARGET_LINK_LIBRARIES(test_wolf_tree ${PROJECT_NAME}) - -# test ceres manager, wolf manager and wolf tree -#ADD_EXECUTABLE(test_ceres_manager_tree test_ceres_manager_tree.cpp) -#TARGET_LINK_LIBRARIES(test_ceres_manager_tree ${PROJECT_NAME}) - -# test ceres covariance -#ADD_EXECUTABLE(test_ceres_covariance test_ceres_covariance.cpp) -#TARGET_LINK_LIBRARIES(test_ceres_covariance ${PROJECT_NAME}) +ADD_EXECUTABLE(test_wolf_tree test_wolf_tree.cpp) +TARGET_LINK_LIBRARIES(test_wolf_tree ${PROJECT_NAME}) # IF (laser_scan_utils_FOUND) # ADD_EXECUTABLE(test_capture_laser_2D test_capture_laser_2D.cpp) @@ -87,7 +50,7 @@ IF(faramotics_FOUND) ${PROJECT_NAME}) IF(Suitesparse_FOUND) - ADD_EXECUTABLE(test_iQR_wolf2 test_iQR_wolf2.cpp) + ADD_EXECUTABLE(test_iQR_wolf2 solver/test_iQR_wolf2.cpp) TARGET_LINK_LIBRARIES(test_iQR_wolf2 ${pose_state_time_LIBRARIES} ${faramotics_LIBRARIES} diff --git a/src/examples/test_SPQR.cpp b/src/examples/solver/test_SPQR.cpp similarity index 100% rename from src/examples/test_SPQR.cpp rename to src/examples/solver/test_SPQR.cpp diff --git a/src/examples/test_ccolamd.cpp b/src/examples/solver/test_ccolamd.cpp similarity index 100% rename from src/examples/test_ccolamd.cpp rename to src/examples/solver/test_ccolamd.cpp diff --git a/src/examples/test_ccolamd_blocks.cpp b/src/examples/solver/test_ccolamd_blocks.cpp similarity index 100% rename from src/examples/test_ccolamd_blocks.cpp rename to src/examples/solver/test_ccolamd_blocks.cpp diff --git a/src/examples/test_iQR.cpp b/src/examples/solver/test_iQR.cpp similarity index 100% rename from src/examples/test_iQR.cpp rename to src/examples/solver/test_iQR.cpp diff --git a/src/examples/test_iQR_wolf.cpp b/src/examples/solver/test_iQR_wolf.cpp similarity index 100% rename from src/examples/test_iQR_wolf.cpp rename to src/examples/solver/test_iQR_wolf.cpp diff --git a/src/examples/test_iQR_wolf2.cpp b/src/examples/solver/test_iQR_wolf2.cpp similarity index 98% rename from src/examples/test_iQR_wolf2.cpp rename to src/examples/solver/test_iQR_wolf2.cpp index 17db520b4767a2239a9d8a58508d85e716b06949..a75890a5a1079db6836b04e711a28f78372f96b2 100644 --- a/src/examples/test_iQR_wolf2.cpp +++ b/src/examples/solver/test_iQR_wolf2.cpp @@ -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(PO_2D, 1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01); - WolfManager* wolf_manager_ceres = new WolfManager(PO_2D, 1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01); + WolfManager* wolf_manager_QR = new WolfManager(PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01); + WolfManager* wolf_manager_ceres = new WolfManager(PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, n_execution*10, 0.01); // Ceres initialization ceres::Solver::Options ceres_options; diff --git a/src/examples/test_incremental_ccolamd_blocks.cpp b/src/examples/solver/test_incremental_ccolamd_blocks.cpp similarity index 100% rename from src/examples/test_incremental_ccolamd_blocks.cpp rename to src/examples/solver/test_incremental_ccolamd_blocks.cpp diff --git a/src/examples/test_permutations.cpp b/src/examples/solver/test_permutations.cpp similarity index 100% rename from src/examples/test_permutations.cpp rename to src/examples/solver/test_permutations.cpp diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp index 482dddd4cb62b4dbc82335621b56edf5fddbfbe3..b691262ccbf8a51e91e9dbdae1f85ed926983774 100644 --- a/src/examples/test_ceres_2_lasers.cpp +++ b/src/examples/test_ceres_2_lasers.cpp @@ -166,7 +166,7 @@ int main(int argc, char** argv) ground_truth.head(3) = pose_odom; odom_trajectory.head(3) = pose_odom; - WolfManager* wolf_manager = new WolfManager(PO_2D, 1e3, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3); + WolfManager* wolf_manager = new WolfManager(PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3); // Ceres wrapper ceres::Solver::Options ceres_options; diff --git a/src/examples/test_ceres_covariance.cpp b/src/examples/test_ceres_covariance.cpp deleted file mode 100644 index 0c4e6d281d517da093737a73fd328c013dcabeb8..0000000000000000000000000000000000000000 --- a/src/examples/test_ceres_covariance.cpp +++ /dev/null @@ -1,103 +0,0 @@ -//std includes -#include <iostream> -#include <memory> -#include <random> - -// Eigen includes -#include <eigen3/Eigen/Dense> -#include <eigen3/Eigen/Geometry> - -//Ceres includes -#include "ceres/ceres.h" -#include "glog/logging.h" - -class rangeBetweenPoints -{ - private: - double d_; - - public: - rangeBetweenPoints(double d): - d_(d) - { - // - } - - template <typename T> - bool operator()(const T* const x , const T* const y, T* e) const - { - e[0] = T(d_) - ((x[0] - y[0])*(x[0] - y[0]) + (x[1] * y[1])*(x[1] * y[1])); - return true; - } -}; - -//This main is a single iteration of the WOLF. -//Once at ROS, Calls to WolfVehicle, CeresWolfFunctor and Ceres objects will be executed in a similar order in a function of the ROS wrapper -int main(int argc, char** argv) -{ - std::cout << " ========= Static Numeric case ===========" << std::endl << std::endl; - - //dimension - const unsigned int DIM = 19; //just to test, all will be DIM-dimensional - - // init - google::InitGoogleLogging(argv[0]); - - // build problem - double x[2]; - double y[2]; - x[0] = 0; - x[1] = 0; - y[0] = 1; - y[1] = 1; - ceres::Problem problem; - problem.AddParameterBlock(x, 2); - problem.AddParameterBlock(y, 2); - std::vector<double*> state_blocks({&x[0], &y[0]}); - - // cost function - ceres::CostFunction* range_cost_function1 = new ceres::AutoDiffCostFunction<rangeBetweenPoints, 1, 2, 2>(new rangeBetweenPoints(1.0)); - ceres::CostFunction* range_cost_function2 = new ceres::AutoDiffCostFunction<rangeBetweenPoints, 1, 2, 2>(new rangeBetweenPoints(1.1)); - - // Add residual block - problem.AddResidualBlock(range_cost_function1, nullptr, state_blocks); - problem.AddResidualBlock(range_cost_function2, nullptr, state_blocks); - - // Solve - ceres::Solver::Options solver_options; - ceres::Solver::Summary summary; - ceres::Solve(solver_options, &problem, &summary); - - //display results - std::cout << "Ceres Report:" << std::endl; - std::cout << summary.FullReport() << "\n"; - - // Compute covariance - ceres::Covariance::Options covariance_options; - ceres::Covariance covariance(covariance_options); - - std::vector<std::pair<const double*, const double*> > covariance_blocks; - covariance_blocks.push_back(std::make_pair(x, x)); - covariance_blocks.push_back(std::make_pair(y, y)); - covariance_blocks.push_back(std::make_pair(x, y)); - - CHECK(covariance.Compute(covariance_blocks, &problem)); - - double covariance_xx[3 * 3]; - double covariance_yy[2 * 2]; - double covariance_xy[3 * 2]; - covariance.GetCovarianceBlock(x, x, covariance_xx); - covariance.GetCovarianceBlock(y, y, covariance_yy); - covariance.GetCovarianceBlock(x, y, covariance_xy); - - std::cout << "Covariances:" << std::endl; - std::cout << "Marginal x:" << covariance_xx << std::endl; - std::cout << "Marginal y:" << covariance_yy << std::endl; - std::cout << "Cross xy:" << covariance_xy << std::endl; - - std::cout << " ========= END ===========" << std::endl << std::endl; - - //exit - return 0; -} - diff --git a/src/examples/test_ceres_laser.cpp b/src/examples/test_ceres_laser.cpp deleted file mode 100644 index a63dd899579855f3fb4c699051ef87aad7f3981b..0000000000000000000000000000000000000000 --- a/src/examples/test_ceres_laser.cpp +++ /dev/null @@ -1,584 +0,0 @@ -//std includes -#include <cstdlib> -#include <iostream> -#include <fstream> -#include <memory> -#include <random> -#include <typeinfo> -#include <ctime> -#include <queue> - -// Eigen includes -#include <eigen3/Eigen/Dense> -#include <eigen3/Eigen/Geometry> - -//Ceres includes -#include "ceres/ceres.h" -#include "glog/logging.h" - -//Wolf includes -#include "wolf.h" -#include "sensor_base.h" -#include "sensor_odom_2D.h" -#include "sensor_gps_fix.h" -#include "feature_base.h" -#include "frame_base.h" -#include "state_point.h" -#include "state_complex_angle.h" -#include "capture_base.h" -#include "capture_relative.h" -#include "capture_odom_2D.h" -#include "capture_gps_fix.h" -#include "capture_laser_2D.h" -#include "state_base.h" -#include "constraint_sparse.h" -#include "constraint_gps_2D.h" -#include "constraint_odom_2D_theta.h" -#include "constraint_odom_2D_complex_angle.h" -#include "trajectory_base.h" -#include "map_base.h" -#include "wolf_problem.h" - -// ceres wrapper include -#include "ceres_wrapper/ceres_manager.h" - -//C includes for sleep, time and main args -#include "unistd.h" - -//faramotics includes -#include "faramotics/dynamicSceneRender.h" -#include "faramotics/rangeScan2D.h" -#include "btr-headers/pose3d.h" - -//laser_scan_utils -#include "iri-algorithms/laser_scan_utils/corner_detector.h" -#include "iri-algorithms/laser_scan_utils/entities.h" - -//function travel around -void motionCampus(unsigned int ii, Cpose3d & pose, double& displacement_, double& rotation_) -{ - if (ii<=120) - { - displacement_ = 0.1; - rotation_ = 0; - } - else if ( (ii>120) && (ii<=170) ) - { - displacement_ = 0.2; - rotation_ = 1.8*M_PI/180; - } - else if ( (ii>170) && (ii<=220) ) - { - displacement_ = 0; - rotation_ = -1.8*M_PI/180; - } - else if ( (ii>220) && (ii<=310) ) - { - displacement_ = 0.1; - rotation_ = 0; - } - else if ( (ii>310) && (ii<=487) ) - { - displacement_ = 0.1; - rotation_ = -1.*M_PI/180; - } - else if ( (ii>487) && (ii<=600) ) - { - displacement_ = 0.2; - rotation_ = 0; - } - else if ( (ii>600) && (ii<=700) ) - { - displacement_ = 0.1; - rotation_ = -1.*M_PI/180; - } - else if ( (ii>700) && (ii<=780) ) - { - displacement_ = 0; - rotation_ = -1.*M_PI/180; - } - else - { - displacement_ = 0.3; - rotation_ = 0.0*M_PI/180; - } - - pose.moveForward(displacement_); - pose.rt.setEuler( pose.rt.head()+rotation_, pose.rt.pitch(), pose.rt.roll() ); -} - -class WolfManager -{ - protected: - bool use_complex_angles_; - RadarOdom+* problem_; - std::queue<CaptureBase*> new_captures_; - SensorBase* sensor_prior_; - unsigned int window_size_; - FrameBaseIter first_window_frame_; - - public: - WolfManager(SensorBase* _sensor_prior, const bool _complex_angle, const unsigned int& _state_length, const unsigned int& _w_size=10) : - use_complex_angles_(_complex_angle), - problem_(new RadarOdom+(_state_length)), - sensor_prior_(_sensor_prior), - window_size_(_w_size) - { - Eigen::VectorXs init_frame(use_complex_angles_ ? 4 : 3); - if (use_complex_angles_) - init_frame << 2, 8, 1, 0; - else - init_frame << 2, 8, 0; - createFrame(init_frame, 0); - } - - virtual ~WolfManager() - { - delete problem_; - } - - void createFrame(const Eigen::VectorXs& _frame_state, const TimeStamp& _time_stamp) - { - // Create frame and add it to the trajectory - StateBlock* new_position = new StateBlock(problem_->getNewStatePtr()); - problem_->addState(new_position, _frame_state.head(2)); - - StateBlock* new_orientation; - if (use_complex_angles_) - new_orientation = new StateComplexAngle(problem_->getNewStatePtr()); - else - new_orientation = new StateBlock(problem_->getNewStatePtr()); - - problem_->addState(new_orientation, _frame_state.tail(new_orientation->getSize())); - - problem_->getTrajectoryPtr()->addFrame(new FrameBase(_time_stamp, new_position, new_orientation)); - - // add a zero odometry capture (in order to integrate) - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new CaptureOdom2D(_time_stamp, - sensor_prior_, - Eigen::Vector2s::Zero(), - Eigen::Matrix2s::Zero())); - } - - void addCapture(CaptureBase* _capture) - { - new_captures_.push(_capture); - std::cout << "added new capture: " << _capture->nodeId() << std::endl; - } - - void update() - { - while (!new_captures_.empty()) - { - // EXTRACT NEW CAPTURE - CaptureBase* new_capture = new_captures_.front(); - new_captures_.pop(); - - // ODOMETRY SENSOR - if (new_capture->getSensorPtr() == sensor_prior_) - { - // FIND PREVIOUS RELATIVE CAPTURE - CaptureMotion* previous_relative_capture = nullptr; - for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin(); capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++) - { - //std::cout << "capture: " << (*capture_it)->nodeId() << std::endl; - if ((*capture_it)->getSensorPtr() == sensor_prior_) - { - previous_relative_capture = (CaptureMotion*)(*capture_it); - break; - } - } - - // INTEGRATING ODOMETRY CAPTURE & COMPUTING PRIOR - std::cout << "integrating captures " << previous_relative_capture->nodeId() << " " << new_capture->nodeId() << std::endl; - previous_relative_capture->integrateCapture((CaptureMotion*)(new_capture)); - Eigen::VectorXs prior = previous_relative_capture->computePrior(); - - // NEW KEY FRAME (if enough time from last frame) - std::cout << "new TimeStamp - last Frame TimeStamp = " << new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() << std::endl; - if (new_capture->getTimeStamp().get() - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getTimeStamp().get() > 0.1) - { - auto previous_frame_ptr = problem_->getTrajectoryPtr()->getFrameListPtr()->back(); - - // NEW FRAME - createFrame(prior, new_capture->getTimeStamp()); - - // COMPUTE PREVIOUS FRAME CAPTURES - for (auto capture_it = previous_frame_ptr->getCaptureListPtr()->begin(); capture_it != previous_frame_ptr->getCaptureListPtr()->end(); capture_it++) - (*capture_it)->processCapture(); - - // WINDOW of FRAMES (remove or fix old frames) - if (problem_->getTrajectoryPtr()->getFrameListPtr()->size() > window_size_) - { - //problem_->getTrajectoryPtr()->removeFrame(problem_->getTrajectoryPtr()->getFrameListPtr()->begin()); - (*first_window_frame_)->fix(); - first_window_frame_++; - } - else - first_window_frame_ = problem_->getTrajectoryPtr()->getFrameListPtr()->begin(); - } - } - else - { - // ADD CAPTURE TO THE LAST FRAME (or substitute the same sensor previous capture) - std::cout << "adding not odometry capture..." << std::endl; - for (auto capture_it = problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->begin(); capture_it != problem_->getTrajectoryPtr()->getFrameListPtr()->back()->getCaptureListPtr()->end(); capture_it++) - { - if ((*capture_it)->getSensorPtr() == new_capture->getSensorPtr()) - { - //std::cout << "removing previous capture..." << std::endl; - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->removeCapture(capture_it); - //std::cout << "removed!" << std::endl; - break; - } - } - problem_->getTrajectoryPtr()->getFrameListPtr()->back()->addCapture(new_capture); - } - } - } - - const Eigen::VectorXs getState() const - { - return problem_->getState(); - } - - StateBlockList* getStateListPtr() - { - return problem_->getStateListPtr(); - } - - void getConstraintsList(ConstraintBaseList& corr_list) - { - problem_->getTrajectoryPtr()->getConstraintList(corr_list); - } - - RadarOdom+* getProblemPtr() - { - return problem_; - } - - void printTree() - { - problem_->print(); - } -}; - -int main(int argc, char** argv) -{ - std::cout << "\n ========= 2D Robot with odometry and GPS ===========\n"; - - // USER INPUT ============================================================================================ - if (argc!=4 || atoi(argv[1])<1 || atoi(argv[1])>1100 || atoi(argv[2]) < 0 || atoi(argv[3]) < 0 || atoi(argv[3]) > 1) - { - std::cout << "Please call me with: [./test_ceres_manager NI PRINT ORIENTATION_MODE], where:" << std::endl; - std::cout << " - NI is the number of iterations (0 < NI < 1100)" << std::endl; - std::cout << " - WS is the window size (0 < WS)" << std::endl; - std::cout << " - ORIENTATION_MODE: 0 for theta, 1 for complex angle" << std::endl; - std::cout << "EXIT due to bad user input" << std::endl << std::endl; - return -1; - } - - clock_t t1, t2; - t1=clock(); - - unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution - unsigned int window_size = (unsigned int) atoi(argv[2]); - bool complex_angle = (bool) atoi(argv[3]); - - // INITIALIZATION ============================================================================================ - //init random generators - WolfScalar odom_std= 0.01; - WolfScalar gps_std= 1; - std::default_random_engine generator(1); - std::normal_distribution<WolfScalar> distribution_odom(0.001, odom_std); //odometry noise - std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise - - //init google log - google::InitGoogleLogging(argv[0]); - - // Ceres initialization - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION; - ceres_options.max_line_search_step_contraction = 1e-3; - // ceres_options.minimizer_progress_to_stdout = false; - // ceres_options.line_search_direction_type = ceres::LBFGS; - // ceres_options.max_num_iterations = 100; - ceres::Problem::Options problem_options; - problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - CeresManager* ceres_manager = new CeresManager(problem_options); - std::ofstream log_file, landmark_file; //output file - - // Faramotics stuff - Cpose3d viewPoint; - Cpose3d devicePose; - vector<Cpose3d> devicePoses; - vector<float> myScan; - string modelFileName; - - //model and initial view point - //modelFileName = "/home/jvallve/iri-lab/faramotics/models/campusNordUPC.obj"; - //modelFileName = "/home/acoromin/dev/br/faramotics/models/campusNordUPC.obj"; - modelFileName = "/home/andreu/dev/faramotics/models/campusNordUPC.obj"; - devicePose.setPose(2,8,0.2,0,0,0); - //glut initialization - //glutInit(&argc, argv); - faramotics::initGLUT(argc, argv); - - //create a viewer for the 3D model and scan points - CdynamicSceneRender* myRender = new CdynamicSceneRender(1200,700,90*M_PI/180,90*700.0*M_PI/(1200.0*180.0),0.2,100); - myRender->loadAssimpModel(modelFileName,true); //with wireframe - //create scanner and load 3D model - CrangeScan2D* myScanner = new CrangeScan2D(HOKUYO_UTM30LX_180DEG);//HOKUYO_UTM30LX_180DEG or LEUZE_RS4 - myScanner->loadAssimpModel(modelFileName); - - //variables - Eigen::Vector2s odom_reading, gps_fix_reading; - Eigen::VectorXs odom_inc_true(n_execution*2);//invented motion - 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 odom_readings(n_execution*2); // all odometry readings - Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings - TimeStamp time_stamp; - Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7); - - // Wolf manager initialization - SensorOdom2D odom_sensor(Eigen::MatrixXs::Zero(6,1), odom_std, odom_std); - //SensorGPSFix gps_sensor(Eigen::MatrixXs::Zero(6,1), gps_std); - Eigen::VectorXs laser_pose(6); - laser_pose << 0,0,0,0,0,0; //origin, no rotation - //SensorLaser2D laser_sensor(Eigen::MatrixXs::Zero(6,1),-M_PI/2, M_PI/2, M_PI/720, 0.2, 100.0, 0.01);//OLD FASHION - SensorLaser2D laser_sensor(Eigen::MatrixXs::Zero(6,1)); - WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, 1e9, window_size); - - // Initial pose - pose_odom << 2,8,0; - ground_truth.head(3) = pose_odom; - odom_trajectory.head(3) = pose_odom; - - std::cout << "START TRAJECTORY..." << std::endl; - // START TRAJECTORY ============================================================================================ - for (unsigned int step=1; step < n_execution; step++) - { - //get init time - t2=clock(); - - // ROBOT MOVEMENT --------------------------- - //std::cout << "ROBOT MOVEMENT..." << std::endl; - // moves the device position - t1=clock(); - motionCampus(step, devicePose, odom_reading(0), odom_reading(1)); - devicePoses.push_back(devicePose); - - - // SENSOR DATA --------------------------- - //std::cout << "SENSOR DATA..." << std::endl; - // store groundtruth - ground_truth.segment(step*3,3) << devicePose.pt(0), devicePose.pt(1), devicePose.rt.head(); - - // compute odometry - odom_reading(0) += distribution_odom(generator); - odom_reading(1) += distribution_odom(generator); - - // odometry integration - pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2)); - pose_odom(1) = pose_odom(1) + odom_reading(0) * sin(pose_odom(2)); - pose_odom(2) = pose_odom(2) + odom_reading(1); - odom_trajectory.segment(step*3,3) = pose_odom; - - // compute GPS - //gps_fix_reading << devicePose.pt(0), devicePose.pt(1), 0; - //gps_fix_reading(0) += distribution_gps(generator); - //gps_fix_reading(1) += distribution_gps(generator); - - //compute scan - myScan.clear(); - myScanner->computeScan(devicePose,myScan); - mean_times(0) += ((double)clock()-t1)/CLOCKS_PER_SEC; - - - // ADD CAPTURES --------------------------- - //std::cout << "ADD CAPTURES..." << std::endl; - t1=clock(); - // adding new sensor captures - time_stamp.setToNow(); - wolf_manager->addCapture(new CaptureOdom2D(time_stamp, &odom_sensor, odom_reading, odom_std * Eigen::MatrixXs::Identity(2,2))); - //wolf_manager->addCapture(new CaptureGPSFix(time_stamp, &gps_sensor, gps_fix_reading, gps_std * MatrixXs::Identity(3,3))); - wolf_manager->addCapture(new CaptureLaser2D(time_stamp, &laser_sensor, myScan)); - - // updating problem - wolf_manager->update(); - mean_times(1) += ((double)clock()-t1)/CLOCKS_PER_SEC; - - - // UPDATING CERES --------------------------- - //std::cout << "UPDATING CERES..." << std::endl; - t1=clock(); - // update state units and constraints in ceres - ceres_manager->update(wolf_manager->getProblemPtr()); - mean_times(2) += ((double)clock()-t1)/CLOCKS_PER_SEC; - - - // SOLVE OPTIMIZATION --------------------------- - //std::cout << "SOLVING..." << std::endl; - t1=clock(); - ceres::Solver::Summary summary = ceres_manager->solve(ceres_options); - //std::cout << summary.FullReport() << std::endl; - mean_times(3) += ((double)clock()-t1)/CLOCKS_PER_SEC; - - // COMPUTE COVARIANCES --------------------------- - //std::cout << "COMPUTING COVARIANCES..." << std::endl; - t1=clock(); -// if (step > 2) -// ceres_manager->computeCovariances(wolf_manager->getProblemPtr()->getStateListPtr(), wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->back()->getPPtr()); - mean_times(4) += ((double)clock()-t1)/CLOCKS_PER_SEC; - - // DRAWING STUFF --------------------------- - t1=clock(); - // draw detected corners - std::list<laserscanutils::Corner> corner_list; - std::vector<double> corner_vector; - CaptureLaser2D last_scan(time_stamp, &laser_sensor, myScan); - last_scan.extractCorners(corner_list); - for (std::list<laserscanutils::Corner>::iterator corner_it = corner_list.begin(); corner_it != corner_list.end(); corner_it++ ) - { - corner_vector.push_back(corner_it->pt_(0)); - corner_vector.push_back(corner_it->pt_(1)); - } - myRender->drawCorners(devicePose,corner_vector); - - // draw landmarks - std::vector<double> landmark_vector; - for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++ ) - { - WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr(); - landmark_vector.push_back(*position_ptr); //x - landmark_vector.push_back(*(position_ptr+1)); //y - landmark_vector.push_back(0.2); //z - } - myRender->drawLandmarks(landmark_vector); - - //Set view point and render the scene - //locate visualization view point, somewhere behind the device - viewPoint.setPose(devicePose); - viewPoint.rt.setEuler( viewPoint.rt.head(), viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() ); - viewPoint.moveForward(-5); - myRender->setViewPoint(viewPoint); - myRender->drawPoseAxis(devicePose); - myRender->drawScan(devicePose,myScan,180.*M_PI/180.,90.*M_PI/180.); //draw scan - myRender->render(); - mean_times(5) += ((double)clock()-t1)/CLOCKS_PER_SEC; - - // TIME MANAGEMENT --------------------------- - mean_times(6) += ((double)clock()-t2)/CLOCKS_PER_SEC; - - if (mean_times(5) < 0.1) - usleep(100000-1e-6*mean_times(5)); - -// std::cout << "\nTree after step..." << std::endl; -// wolf_manager->printTree(); - } - - // DISPLAY RESULTS ============================================================================================ - mean_times /= n_execution; - std::cout << "\nSIMULATION AVERAGE LOOP DURATION [s]" << std::endl; - std::cout << " data generation: " << mean_times(0) << std::endl; - std::cout << " wolf managing: " << mean_times(1) << std::endl; - std::cout << " ceres managing: " << mean_times(2) << std::endl; - std::cout << " ceres optimization: " << mean_times(3) << std::endl; - std::cout << " ceres covariance: " << mean_times(4) << std::endl; - std::cout << " results drawing: " << mean_times(5) << std::endl; - std::cout << " loop time: " << mean_times(6) << std::endl; - -// std::cout << "\nTree before deleting..." << std::endl; -// wolf_manager->printTree(); - - // Draw Final result ------------------------- - std::vector<double> landmark_vector; - for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++ ) - { - WolfScalar* position_ptr = (*landmark_it)->getPPtr()->getPtr(); - landmark_vector.push_back(*position_ptr); //x - landmark_vector.push_back(*(position_ptr+1)); //y - landmark_vector.push_back(0.2); //z - } - myRender->drawLandmarks(landmark_vector); - viewPoint.setPose(devicePoses.front()); - viewPoint.moveForward(10); - viewPoint.rt.setEuler( viewPoint.rt.head()+M_PI/4, viewPoint.rt.pitch()+20.*M_PI/180., viewPoint.rt.roll() ); - viewPoint.moveForward(-10); - myRender->setViewPoint(viewPoint); - myRender->render(); - - // Print Final result in a file ------------------------- - // Vehicle poses - int i = 0; - Eigen::VectorXs state_poses(n_execution * 3); - for (auto frame_it = wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->begin(); frame_it != wolf_manager->getProblemPtr()->getTrajectoryPtr()->getFrameListPtr()->end(); frame_it++ ) - { - if (complex_angle) - state_poses.segment(i,3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr()+1), atan2(*(*frame_it)->getOPtr()->getPtr(), *((*frame_it)->getOPtr()->getPtr()+1)); - else - state_poses.segment(i,3) << *(*frame_it)->getPPtr()->getPtr(), *((*frame_it)->getPPtr()->getPtr()+1), *(*frame_it)->getOPtr()->getPtr(); - i+=3; - } - - // Landmarks - i = 0; - Eigen::VectorXs landmarks(wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->size()*2); - for (auto landmark_it = wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->begin(); landmark_it != wolf_manager->getProblemPtr()->getMapPtr()->getLandmarkListPtr()->end(); landmark_it++ ) - { - Eigen::Map<Eigen::Vector2s> landmark((*landmark_it)->getPPtr()->getPtr()); - landmarks.segment(i,2) = landmark; - i+=2; - } - - // Print log files - std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt")); - log_file.open(filepath, std::ofstream::out); //open log file - - if (log_file.is_open()) - { - log_file << 0 << std::endl; - for (unsigned int ii = 0; ii<n_execution; ii++) - log_file << state_poses.segment(ii*3,3).transpose() - << "\t" << ground_truth.segment(ii*3,3).transpose() - << "\t" << (state_poses.segment(ii*3,3)-ground_truth.segment(ii*3,3)).transpose() - << "\t" << odom_trajectory.segment(ii*3,3).transpose() - << "\t" << gps_fix_readings.segment(ii*3,3).transpose() << std::endl; - log_file.close(); //close log file - std::cout << std::endl << "Result file " << filepath << std::endl; - } - else - std::cout << std::endl << "Failed to write the log file " << filepath << std::endl; - - std::string filepath2 = getenv("HOME") + (complex_angle ? std::string("/Desktop/landmarks_file_3.txt") : std::string("/Desktop/landmarks_file_2.txt")); - landmark_file.open(filepath2, std::ofstream::out); //open log file - - if (landmark_file.is_open()) - { - for (unsigned int ii = 0; ii<landmarks.size(); ii+=2) - landmark_file << landmarks.segment(ii,2).transpose() << std::endl; - landmark_file.close(); //close log file - std::cout << std::endl << "Landmark file " << filepath << std::endl; - } - else - std::cout << std::endl << "Failed to write the landmark file " << filepath << std::endl; - - std::cout << "Press any key for ending... " << std::endl << std::endl; - std::getchar(); - - delete myRender; - delete myScanner; - delete wolf_manager; - std::cout << "wolf deleted" << std::endl; - delete ceres_manager; - std::cout << "ceres_manager deleted" << std::endl; - - std::cout << " ========= END ===========" << std::endl << std::endl; - - //exit - return 0; -} diff --git a/src/examples/test_ceres_manager.cpp b/src/examples/test_ceres_manager.cpp deleted file mode 100644 index 65221225f234122e5d6f2f2c3e56fce9610fc7b4..0000000000000000000000000000000000000000 --- a/src/examples/test_ceres_manager.cpp +++ /dev/null @@ -1,844 +0,0 @@ -//std includes -#include <cstdlib> -#include <iostream> -#include <fstream> -#include <memory> -#include <random> -#include <typeinfo> -#include <ctime> -#include <queue> - -// Eigen includes -#include <eigen3/Eigen/Dense> -#include <eigen3/Eigen/Geometry> - -//Ceres includes -#include "ceres/jet.h" -#include "ceres/ceres.h" -#include "glog/logging.h" - -//Wolf includes -#include "sensor_base.h" -#include "frame_base.h" -#include "state_point.h" -#include "state_complex_angle.h" -#include "state_theta.h" -#include "capture_base.h" -#include "state_base.h" -#include "wolf.h" - -// ceres wrapper includes -#include "ceres_wrapper/complex_angle_parametrization.h" - -/** - * This test implements an optimization using CERES of a vehicle trajectory using odometry and GPS simulated data. - * - **/ - -using namespace Eigen; - -class ConstraintXBase -{ - protected: - WolfScalar *measurement_ptr_; - - public: - - ConstraintXBase(WolfScalar * _measurement_ptr) : - measurement_ptr_(_measurement_ptr) - { - } - - virtual ~ConstraintXBase() - { - } - - virtual ConstraintType getType() const = 0; - virtual const std::vector<WolfScalar*> getBlockPtrVector() = 0; -}; - -template <const unsigned int MEASUREMENT_SIZE, - unsigned int BLOCK_0_SIZE, - unsigned int BLOCK_1_SIZE = 0, - unsigned int BLOCK_2_SIZE = 0, - unsigned int BLOCK_3_SIZE = 0, - unsigned int BLOCK_4_SIZE = 0, - unsigned int BLOCK_5_SIZE = 0, - unsigned int BLOCK_6_SIZE = 0, - unsigned int BLOCK_7_SIZE = 0, - unsigned int BLOCK_8_SIZE = 0, - unsigned int BLOCK_9_SIZE = 0> -class ConstraintSparse: public ConstraintXBase -{ - protected: - std::vector<WolfScalar*> state_block_ptr_vector_; - std::vector<unsigned int> block_sizes_vector_; - - public: - static const unsigned int measurementSize = MEASUREMENT_SIZE; - static const unsigned int block0Size = BLOCK_0_SIZE; - static const unsigned int block1Size = BLOCK_1_SIZE; - static const unsigned int block2Size = BLOCK_2_SIZE; - static const unsigned int block3Size = BLOCK_3_SIZE; - static const unsigned int block4Size = BLOCK_4_SIZE; - static const unsigned int block5Size = BLOCK_5_SIZE; - static const unsigned int block6Size = BLOCK_6_SIZE; - static const unsigned int block7Size = BLOCK_7_SIZE; - static const unsigned int block8Size = BLOCK_8_SIZE; - static const unsigned int block9Size = BLOCK_9_SIZE; - - ConstraintSparse(WolfScalar* _measurementPtr, WolfScalar** _blockPtrArray) : - ConstraintXBase(_measurementPtr), - block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE}) - { - for (unsigned int i = 0; i<block_sizes_vector_.size(); i++) - { - if (block_sizes_vector_.at(i) == 0) - { - block_sizes_vector_.resize(i); - break; - } - else - state_block_ptr_vector_.push_back(_blockPtrArray[i]); - } - } - - ConstraintSparse(WolfScalar* _measurementPtr, - WolfScalar* _state0Ptr, - WolfScalar* _state1Ptr = nullptr, - WolfScalar* _state2Ptr = nullptr, - WolfScalar* _state3Ptr = nullptr, - WolfScalar* _state4Ptr = nullptr, - WolfScalar* _state5Ptr = nullptr, - WolfScalar* _state6Ptr = nullptr, - WolfScalar* _state7Ptr = nullptr, - WolfScalar* _state8Ptr = nullptr, - WolfScalar* _state9Ptr = nullptr ) : - ConstraintXBase(_measurementPtr), - state_block_ptr_vector_({_state0Ptr,_state1Ptr,_state2Ptr,_state3Ptr,_state4Ptr,_state5Ptr,_state6Ptr,_state7Ptr,_state8Ptr,_state9Ptr}), - block_sizes_vector_({BLOCK_0_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE,BLOCK_3_SIZE,BLOCK_4_SIZE,BLOCK_5_SIZE,BLOCK_6_SIZE,BLOCK_7_SIZE,BLOCK_8_SIZE,BLOCK_9_SIZE}) - { - for (unsigned int i = 0; i<block_sizes_vector_.size(); i++) - { - if (block_sizes_vector_.at(i) == 0) - { - block_sizes_vector_.resize(i); - state_block_ptr_vector_.resize(i); - break; - } - } - - //TODO: Check if while size OK, pointers NULL - } - - virtual ~ConstraintSparse() - { - } - - virtual ConstraintType getType() const = 0; - - virtual const std::vector<WolfScalar *> getBlockPtrVector() - { - return state_block_ptr_vector_; - } -}; - -class ConstraintGPS2D : public ConstraintSparse<2,2> -{ - public: - static const unsigned int N_BLOCKS = 1; - const double stdev_ = 1; - - ConstraintGPS2D(WolfScalar* _measurementPtr, WolfScalar* _statePtr) : - ConstraintSparse<2,2>(_measurementPtr, _statePtr) - { - } - - ConstraintGPS2D(WolfScalar* _measurementPtr, StateBlock* _statePtr) : - ConstraintSparse<2,2>(_measurementPtr, _statePtr->getPtr()) - { - } - - virtual ~ConstraintGPS2D() - { - } - - template <typename T> - bool operator()(const T* const _x, T* _residuals) const - { - _residuals[0] = (T(*this->measurement_ptr_) - _x[0]) / T(stdev_); - _residuals[1] = (T(*(this->measurement_ptr_+1)) - _x[1]) / T(stdev_); - return true; - } - - virtual ConstraintType getType() const - { - return CTR_GPS_FIX_2D; - } -}; - -class Constraint2DOdometry : public ConstraintSparse<2,2,2,2,2> -{ - public: - static const unsigned int N_BLOCKS = 4; - const double stdev_ = 0.01; //model parameters - - Constraint2DOdometry(WolfScalar* _measurementPtr, WolfScalar** _blockPtrs) : - ConstraintSparse<2,2,2,2,2>(_measurementPtr, _blockPtrs) - { - } - - Constraint2DOdometry(WolfScalar* _measurementPtr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) : - ConstraintSparse<2,2,2,2,2>(_measurementPtr, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr) - { - } - - Constraint2DOdometry(WolfScalar* _measurementPtr, StateBlock* _state0Ptr, StateBlock* _state1Ptr, StateBlock* _state2Ptr, StateBlock* _state3Ptr) : - ConstraintSparse<2,2,2,2,2>(_measurementPtr, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr()) - { - } - - virtual ~Constraint2DOdometry() - { - } - - template <typename T> - bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const - { - // Expected measurement - T expected_range = (_p1[0]-_p2[0])*(_p1[0]-_p2[0]) + (_p1[1]-_p2[1])*(_p1[1]-_p2[1]); //square of the range - T expected_rotation = atan2(_o2[1]*_o1[0] - _o2[0]*_o1[1], _o1[0]*_o2[0] + _o1[1]*_o2[1]); - //T expected_rotation = atan2(_o2[1], _o2[0]) -atan2(_o1[1],_o1[0]); - - // Residuals - _residuals[0] = (expected_range - T((*this->measurement_ptr_)*(*this->measurement_ptr_))) / T(stdev_); - _residuals[1] = (expected_rotation - T(*(this->measurement_ptr_+1))) / T(stdev_); - - return true; - } - - virtual ConstraintType getType() const - { - return CTR_ODOM_2D_COMPLEX_ANGLE; - } -}; - -class Constraint2DOdometryTheta : public ConstraintSparse<2,2,1,2,1> -{ - public: - static const unsigned int N_BLOCKS = 4; - const double stdev_ = 0.01; //model parameters - - Constraint2DOdometryTheta(WolfScalar* _measurementPtr, WolfScalar** _blockPtrs) : - ConstraintSparse<2,2,1,2,1>(_measurementPtr, _blockPtrs) - { - } - - Constraint2DOdometryTheta(WolfScalar* _measurementPtr, WolfScalar* _block0Ptr, WolfScalar* _block1Ptr, WolfScalar* _block2Ptr, WolfScalar* _block3Ptr) : - ConstraintSparse<2,2,1,2,1>(_measurementPtr, _block0Ptr, _block1Ptr, _block2Ptr, _block3Ptr) - { - } - - Constraint2DOdometryTheta(WolfScalar* _measurementPtr, StateBlock* _state0Ptr, StateBlock* _state1Ptr, StateBlock* _state2Ptr, StateBlock* _state3Ptr) : - ConstraintSparse<2,2,1,2,1>(_measurementPtr, _state0Ptr->getPtr(), _state1Ptr->getPtr(),_state2Ptr->getPtr(), _state3Ptr->getPtr()) - { - } - - virtual ~Constraint2DOdometryTheta() - { - } - - template <typename T> - bool operator()(const T* const _p1, const T* const _o1, const T* const _p2, const T* const _o2, T* _residuals) const - { - // Expected measurement - T expected_range = (_p2[0]-_p1[0])*(_p2[0]-_p1[0]) + (_p2[1]-_p1[1])*(_p2[1]-_p1[1]); //square of the range - T expected_rotation = _o2[0]-_o1[0]; - - // Residuals - _residuals[0] = (expected_range - T((*this->measurement_ptr_)*(*this->measurement_ptr_))) / T(stdev_); - _residuals[1] = (expected_rotation - T(*(this->measurement_ptr_+1))) / T(stdev_); - - return true; - } - - virtual ConstraintType getType() const - { - return CTR_ODOM_2D; - } -}; - -class CaptureXBase -{ - public: - VectorXs capture; - TimeStamp time_stamp; - SensorBase* sensor_ptr_; ///< Pointer to sensor - - CaptureXBase(const VectorXs& _capture, const WolfScalar& _time_stamp, SensorBase* _sensor_ptr) : - capture(_capture), - time_stamp(_time_stamp), - sensor_ptr_(_sensor_ptr) - { - } - - virtual ~CaptureXBase() - { - } - - const SensorType getSensorType() const - { - return sensor_ptr_->getSensorType(); - } - - WolfScalar* getPtr() - { - return capture.data(); - } -}; - -class WolfManager -{ - protected: - VectorXs state_; - unsigned int first_empty_state_; - bool use_complex_angles_; - std::vector<FrameBase*> frames_; - std::vector<ConstraintXBase*> constraints_; - std::vector<VectorXs> odom_captures_; - std::vector<VectorXs> gps_captures_; - std::queue<CaptureXBase*> new_captures_; - std::vector<CaptureXBase*> captures_; - - public: - WolfManager(const unsigned int& _state_length=1000, const bool _complex_angle=false) : - state_(_state_length), - first_empty_state_(0), - use_complex_angles_(_complex_angle), - frames_(0), - constraints_(0) - { - VectorXs init_frame(use_complex_angles_ ? 4 : 3); - if (use_complex_angles_) - init_frame << 0, 0, 1, 0; - else - init_frame << 0, 0, 0; - createFrame(init_frame, 0); - } - - virtual ~WolfManager() - { -// std::cout << "Destroying WolfManager...\n"; -// std::cout << "Clearing constraints_...\n"; -// constraints_.clear(); -// std::cout << "Clearing frames...\n"; -// frames_.clear(); -// std::cout << "Clearing odom_captures_...\n"; -// odom_captures_.clear(); -// std::cout << "Clearing gps_captures_...\n"; -// gps_captures_.clear(); -// captures_.clear(); -// -// std::cout << "all cleared...\n"; - } - - unsigned int getConstraintsSize() - { - return constraints_.size(); - } - - void createFrame(const VectorXs& _frame_state, const TimeStamp& _time_stamp) - { - // Store in state_ - state_.segment(first_empty_state_, use_complex_angles_ ? 4 : 3) << _frame_state; - - // Create frame - if (use_complex_angles_) -// frames_.push_back(FrameBaseShPtr(new FrameBase(nullptr, _time_stamp, -// StateBaseShPtr(new StateBase(state_.data()+first_empty_state_)), -// StateBaseShPtr(new StateComplexAngle(state_.data()+first_empty_state_+2))))); - frames_.push_back(new FrameBase(_time_stamp, - new StateBlock(state_.data()+first_empty_state_), - new StateComplexAngle(state_.data()+first_empty_state_+2))); - - else -// frames_.push_back(FrameBaseShPtr(new FrameBase(nullptr, _time_stamp, -// StateBaseShPtr(new StateBase(state_.data()+first_empty_state_)), -// StateBaseShPtr(new StateBase(state_.data()+first_empty_state_+2))))); - frames_.push_back(new FrameBase(_time_stamp, - new StateBlock(state_.data()+first_empty_state_), - new StateBlock(state_.data()+first_empty_state_+2))); - // Update first free state location index - first_empty_state_ += use_complex_angles_ ? 4 : 3; - } - - void addCapture(const VectorXs& _odom_capture, const WolfScalar& _time_stamp, SensorBase* _sensor_ptr) - { - new_captures_.push(new CaptureXBase(_odom_capture, _time_stamp, _sensor_ptr)); - } - - void computeOdomCapture(CaptureXBase* _odom_capture) - { - FrameBase* prev_frame_ptr = frames_.back(); - - // STORE CAPTURE - captures_.push_back(_odom_capture); - VectorXs capture_data = _odom_capture->capture; - - // PRIOR - VectorXs pose_predicted(use_complex_angles_ ? 4 : 3); - Map<VectorXs> previous_pose(prev_frame_ptr->getPPtr()->getPtr(), use_complex_angles_ ? 4 : 3); - if (use_complex_angles_) - { - double new_pose_predicted_2 = previous_pose(2) * cos(capture_data(1)) - previous_pose(3) * sin(capture_data(1)); - double new_pose_predicted_3 = previous_pose(2) * sin(capture_data(1)) + previous_pose(3) * cos(capture_data(1)); - pose_predicted(0) = previous_pose(0) + capture_data(0) * new_pose_predicted_2; - pose_predicted(1) = previous_pose(1) + capture_data(0) * new_pose_predicted_3; - pose_predicted(2) = new_pose_predicted_2; - pose_predicted(3) = new_pose_predicted_3; - } - else - { - pose_predicted(0) = previous_pose(0) + capture_data(0) * cos(previous_pose(2) + (capture_data(1))); - pose_predicted(1) = previous_pose(1) + capture_data(0) * sin(previous_pose(2) + (capture_data(1))); - pose_predicted(2) = previous_pose(2) + (capture_data(1)); - } - - // NEW FRAME - createFrame(pose_predicted, _odom_capture->time_stamp); - - // CORRESPONDENCE ODOMETRY - if (use_complex_angles_) - constraints_.push_back(new Constraint2DOdometry(_odom_capture->getPtr(), - prev_frame_ptr->getPPtr()->getPtr(), - prev_frame_ptr->getOPtr()->getPtr(), - frames_.back()->getPPtr()->getPtr(), - frames_.back()->getOPtr()->getPtr())); - - else - constraints_.push_back(new Constraint2DOdometryTheta(_odom_capture->getPtr(), - prev_frame_ptr->getPPtr()->getPtr(), - prev_frame_ptr->getOPtr()->getPtr(), - frames_.back()->getPPtr()->getPtr(), - frames_.back()->getOPtr()->getPtr())); - } - - void computeGPSCapture(CaptureXBase* _gps_capture) - { - // STORE CAPTURE - captures_.push_back(_gps_capture); - - // CORRESPONDENCE GPS - constraints_.push_back(new ConstraintGPS2D(_gps_capture->getPtr(), frames_.back()->getPPtr()->getPtr())); - } - - void update(std::queue<StateBlock*>& new_state_units, std::queue<ConstraintXBase*>& new_constraints) - { - while (!new_captures_.empty()) - { - switch (new_captures_.front()->getSensorType()) - { - case GPS_FIX: - computeGPSCapture(new_captures_.front()); - new_constraints.push(constraints_.back()); - break; - case ODOM_2D: - computeOdomCapture(new_captures_.front()); - new_constraints.push(constraints_.back()); - new_state_units.push(frames_.back()->getPPtr()); - new_state_units.push(frames_.back()->getOPtr()); - break; - default: - std::cout << "unknown capture...\n"; - } - new_captures_.pop(); - } - } - - VectorXs getState() - { - return state_; - } - - ConstraintXBase* getConstraintPrt(unsigned int i) - { - return constraints_.at(i); - } - - std::queue<StateBlock*> getStateUnitsPtrs(unsigned int i) - { - return std::queue<StateBlock*>({frames_.at(i)->getPPtr(),frames_.at(i)->getOPtr()}); - } -}; - -class CeresManager -{ - protected: - - std::vector<std::pair<ceres::ResidualBlockId, ConstraintXBase*>> constraint_list_; - ceres::Problem* ceres_problem_; - - public: - CeresManager(ceres::Problem* _ceres_problem) : - ceres_problem_(_ceres_problem) - { - } - - ~CeresManager() - { -// std::vector<double*> state_units; -// ceres_problem_->GetParameterBlocks(&state_units); -// -// for (unsigned int i = 0; i< state_units.size(); i++) -// removeStateUnit(state_units.at(i)); -// -// std::cout << "all state units removed! \n"; - std::cout << "residuals: " << ceres_problem_->NumResiduals() << "\n"; - std::cout << "parameters: " << ceres_problem_->NumParameters() << "\n"; - } - - ceres::Solver::Summary solve(const ceres::Solver::Options& _ceres_options) - { - // create summary - ceres::Solver::Summary ceres_summary_; - - // run Ceres Solver - ceres::Solve(_ceres_options, ceres_problem_, &ceres_summary_); - - // compute covariances - ceres::Covariance::Options covariance_options; - covariance_options.algorithm_type = ceres::DENSE_SVD; - covariance_options.null_space_rank = -1; - ceres::Covariance covariance_(covariance_options); - - std::vector<double*> parameter_blocks; - ceres_problem_->GetParameterBlocks(¶meter_blocks); - - std::vector<std::pair<const double*, const double*>> covariance_blocks; - covariance_blocks.push_back(std::make_pair(parameter_blocks[0],parameter_blocks[1])); - covariance_.Compute(covariance_blocks, ceres_problem_); - - double covariance_12[3 * 3]; - covariance_.GetCovarianceBlock(parameter_blocks[0], parameter_blocks[1], covariance_12); - - std::cout << "12 cov: " << covariance_12[0] << " " << - covariance_12[1] << " " << - covariance_12[2] << " " << - covariance_12[3] << " " << - covariance_12[4] << " " << - covariance_12[5] << " " << - covariance_12[6] << " " << - covariance_12[7] << " " << - covariance_12[8] << " " <<std::endl; - - //display results - return ceres_summary_; - } - - void addConstraints(std::queue<ConstraintXBase*>& _new_constraints) - { - //std::cout << _new_constraints.size() << " new constraints\n"; - while (!_new_constraints.empty()) - { - addConstraint(_new_constraints.front()); - _new_constraints.pop(); - } - } - - void removeConstraints() - { - for (unsigned int i = 0; i<constraint_list_.size(); i++) - { - ceres_problem_->RemoveResidualBlock(constraint_list_.at(i).first); - } - constraint_list_.clear(); - std::cout << ceres_problem_->NumResidualBlocks() << " residual blocks \n"; - } - - void addConstraint(ConstraintXBase* _corr_ptr) - { - ceres::ResidualBlockId blockIdx = ceres_problem_->AddResidualBlock(createCostFunction(_corr_ptr), NULL, _corr_ptr->getBlockPtrVector()); - constraint_list_.push_back(std::pair<ceres::ResidualBlockId, ConstraintXBase*>(blockIdx,_corr_ptr)); - } - - void addStateUnits(std::queue<StateBlock*>& _new_state_units) - { - while (!_new_state_units.empty()) - { - addStateBlock(_new_state_units.front()); - _new_state_units.pop(); - } - } - - void removeStateUnit(WolfScalar* _st_ptr) - { - ceres_problem_->RemoveParameterBlock(_st_ptr); - } - - void addStateBlock(StateBlock* _st_ptr) - { - //std::cout << "Adding a State Unit to wolf_problem... " << std::endl; - //_st_ptr->print(); - - switch (_st_ptr->getType()) - { - case ST_COMPLEX_ANGLE: - { - //std::cout << "Adding Complex angle Local Parametrization to the List... " << std::endl; - //ceres_problem_->SetParameterization(_st_ptr->getPtr(), new ComplexAngleParameterization); - ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateComplexAngle*)_st_ptr)->BLOCK_SIZE, new ComplexAngleParameterization); - break; - } -// case PARAM_QUATERNION: -// { -// std::cout << "Adding Quaternion Local Parametrization to the List... " << std::endl; -// ceres_problem_->SetParameterization(_st_ptr->getPtr(), new EigenQuaternionParameterization); -// ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateQuaternion*)_st_ptr.get())->BLOCK_SIZE, new QuaternionParameterization); -// break; -// } - case ST_POINT_1D: //equivalent case ST_THETA: - { - //std::cout << "No Local Parametrization to be added" << std::endl; - ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StatePoint1D*)_st_ptr)->BLOCK_SIZE, nullptr); - break; - } - case ST_POINT_2D: - { - //std::cout << "No Local Parametrization to be added" << std::endl; - ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlock*)_st_ptr)->BLOCK_SIZE, nullptr); - break; - } - case ST_POINT_3D: - { - //std::cout << "No Local Parametrization to be added" << std::endl; - ceres_problem_->AddParameterBlock(_st_ptr->getPtr(), ((StateBlock*)_st_ptr)->BLOCK_SIZE, nullptr); - break; - } - default: - std::cout << "Unknown Local Parametrization type!" << std::endl; - } - } - - ceres::CostFunction* createCostFunction(ConstraintXBase* _corrPtr) - { - switch (_corrPtr->getType()) - { - case CTR_GPS_FIX_2D: - { - ConstraintGPS2D* specific_ptr = (ConstraintGPS2D*)(_corrPtr); - return new ceres::AutoDiffCostFunction<ConstraintGPS2D, - specific_ptr->measurementSize, - specific_ptr->block0Size, - specific_ptr->block1Size, - specific_ptr->block2Size, - specific_ptr->block3Size, - specific_ptr->block4Size, - specific_ptr->block5Size, - specific_ptr->block6Size, - specific_ptr->block7Size, - specific_ptr->block8Size, - specific_ptr->block9Size>(specific_ptr); - break; - } - case CTR_ODOM_2D_COMPLEX_ANGLE: - { - Constraint2DOdometry* specific_ptr = (Constraint2DOdometry*)(_corrPtr); - return new ceres::AutoDiffCostFunction<Constraint2DOdometry, - specific_ptr->measurementSize, - specific_ptr->block0Size, - specific_ptr->block1Size, - specific_ptr->block2Size, - specific_ptr->block3Size, - specific_ptr->block4Size, - specific_ptr->block5Size, - specific_ptr->block6Size, - specific_ptr->block7Size, - specific_ptr->block8Size, - specific_ptr->block9Size>(specific_ptr); - break; - } - case CTR_ODOM_2D: - { - Constraint2DOdometryTheta* specific_ptr = (Constraint2DOdometryTheta*)(_corrPtr); - return new ceres::AutoDiffCostFunction<Constraint2DOdometryTheta, - specific_ptr->measurementSize, - specific_ptr->block0Size, - specific_ptr->block1Size, - specific_ptr->block2Size, - specific_ptr->block3Size, - specific_ptr->block4Size, - specific_ptr->block5Size, - specific_ptr->block6Size, - specific_ptr->block7Size, - specific_ptr->block8Size, - specific_ptr->block9Size>(specific_ptr); - break; - } - default: - std::cout << "Unknown constraint type! Please add it in the CeresWrapper::createCostFunction()" << std::endl; - - return nullptr; - } - } -}; - -int main(int argc, char** argv) -{ - std::cout << " ========= 2D Robot with odometry and GPS ===========\n\n"; - - // USER INPUT ============================================================================================ - if (argc!=3 || atoi(argv[1])<1 || atoi(argv[2]) < 0 || atoi(argv[2]) > 1) - { - std::cout << "Please call me with: [./test_ceres_manager NI PRINT ORIENTATION_MODE], where:" << std::endl; - std::cout << " - NI is the number of iterations (NI > 0)" << std::endl; - std::cout << " - ORIENTATION_MODE: 0 for theta, 1 for complex angle" << std::endl; - std::cout << "EXIT due to bad user input" << std::endl << std::endl; - return -1; - } - - clock_t t1, t2; - t1=clock(); - - NodeLinked<NodeTerminus,NodeTerminus> node(TOP,"TRAJECTORY"); - - unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution - bool complex_angle = (bool) atoi(argv[2]); - - // INITIALIZATION ============================================================================================ - //init random generators - std::default_random_engine generator(1); - std::normal_distribution<WolfScalar> distribution_odom(0.001,0.01); //odometry noise - std::normal_distribution<WolfScalar> distribution_gps(0.0,1); //GPS noise - - //init google log - google::InitGoogleLogging(argv[0]); - - // Ceres initialization - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION; - ceres_options.max_line_search_step_contraction = 1e-3; - // ceres_options.minimizer_progress_to_stdout = false; - // ceres_options.line_search_direction_type = ceres::LBFGS; - // ceres_options.max_num_iterations = 100; - ceres::Problem* ceres_problem = new ceres::Problem(); - CeresManager* ceres_manager = new CeresManager(ceres_problem); - std::ofstream log_file; //output file - // Wolf manager initialization - WolfManager* wolf_manager = new WolfManager(n_execution * (complex_angle ? 4 : 3), complex_angle); - - //variables - Eigen::VectorXs odom_inc_true(n_execution*2);//invented motion - Eigen::VectorXs pose_true(3); //current true pose - Eigen::VectorXs pose_odom(3); //current true pose - Eigen::VectorXs ground_truth(n_execution*3); //all true poses - Eigen::VectorXs odom_trajectory(n_execution*3); //all true poses - Eigen::VectorXs odom_readings(n_execution*2); // all odometry readings - Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings - std::queue<StateBlock*> new_state_units; // new state units in wolf that must be added to ceres - std::queue<ConstraintXBase*> new_constraints; // new constraints in wolf that must be added to ceres - SensorBase* odom_sensor = new SensorBase(ODOM_2D, Eigen::MatrixXs::Zero(6,1),0); - SensorBase* gps_sensor = new SensorBase(GPS_FIX, Eigen::MatrixXs::Zero(6,1),0); - - // Initial pose - pose_true << 0,0,0; - pose_odom << 0,0,0; - ground_truth.head(3) = pose_true; - odom_trajectory.head(3) = pose_true; - - // SENSOR DATA ============================================================================================ - for (unsigned int ii = 1; ii<n_execution; ii++) - { - // inventing odometry ground truth - if ( ii < (unsigned int)floor(n_execution/2) ) - odom_inc_true.segment(ii*2,2) << fabs(cos(ii/10.)) , fabs(sin(ii/2000.)); //invented motion increments. - else - odom_inc_true.segment(ii*2,2) << fabs(cos(ii/10.)) , -fabs(sin((ii-floor(n_execution/2))/2000.)); //invented motion increments. - - // Computing ground truth trajectory - pose_true(0) = pose_true(0) + odom_inc_true(ii*2) * cos(pose_true(2) + odom_inc_true(ii*2+1)); - pose_true(1) = pose_true(1) + odom_inc_true(ii*2) * sin(pose_true(2) + odom_inc_true(ii*2+1)); - pose_true(2) = pose_true(2) + odom_inc_true(ii*2+1); - ground_truth.segment(ii*3,3) << pose_true; - - // corrupting sensor readings (odometry and GPS) - odom_readings.segment(ii*2,2) << odom_inc_true(ii*2) + distribution_odom(generator), - odom_inc_true(ii*2+1) + distribution_odom(generator); //true range and theta with noise - gps_fix_readings.segment(ii*3,3) << pose_true(0) + distribution_gps(generator), - pose_true(1) + distribution_gps(generator), - 0. + distribution_gps(generator); - - // Computing ground truth trajectory - pose_odom(0) = pose_odom(0) + odom_readings(ii*2) * cos(pose_odom(2) + odom_readings(ii*2+1)); - pose_odom(1) = pose_odom(1) + odom_readings(ii*2) * sin(pose_odom(2) + odom_readings(ii*2+1)); - pose_odom(2) = pose_odom(2) + odom_readings(ii*2+1); - odom_trajectory.segment(ii*3,3) << pose_odom; - } - std::cout << "sensor data created!\n"; - - // START TRAJECTORY ============================================================================================ - new_state_units = wolf_manager->getStateUnitsPtrs(0); // First pose to be added in ceres - for (unsigned int step=1; step < n_execution; step++) - { - // adding sensor captures - wolf_manager->addCapture(odom_readings.segment(step*2,2),step,odom_sensor); - wolf_manager->addCapture(gps_fix_readings.segment(step*3,3),step,gps_sensor); - - // updating problem - wolf_manager->update(new_state_units, new_constraints); - - // adding new state units and constraints to ceres - ceres_manager->addStateUnits(new_state_units); - ceres_manager->addConstraints(new_constraints); - } - - // SOLVE OPTIMIZATION ============================================================================================ - ceres::Solver::Summary summary = ceres_manager->solve(ceres_options); - t2=clock(); - double seconds = ((double)t2-t1)/CLOCKS_PER_SEC; - - // DISPLAY RESULTS ============================================================================================ - std::cout << summary.FullReport() << std::endl; - std::cout << "optimization seconds: " << summary.total_time_in_seconds << std::endl; - std::cout << "total seconds: " << seconds << std::endl; - - // change from complex angle to theta - VectorXs state = wolf_manager->getState(); - VectorXs state_theta(n_execution * 3); - if (complex_angle) - for (unsigned int ii = 0; ii<n_execution; ii++) - state_theta.segment(ii*3,3) << state(ii*4), state(ii*4+1), atan2(state(ii*4+2), state(ii*4+3)); - else - state_theta = state; - - // Print log file - std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt")); - log_file.open(filepath, std::ofstream::out); //open log file - - if (log_file.is_open()) - { - log_file << seconds << std::endl; - for (unsigned int ii = 0; ii<n_execution; ii++) - log_file << state_theta.segment(ii*3,3).transpose() - << "\t" << ground_truth.segment(ii*3,3).transpose() - << "\t" << (state_theta.segment(ii*3,3)-ground_truth.segment(ii*3,3)).transpose() - << "\t" << odom_trajectory.segment(ii*3,3).transpose() - << "\t" << gps_fix_readings.segment(ii*3,3).transpose() << std::endl; - log_file.close(); //close log file - std::cout << std::endl << "Result file " << filepath << std::endl; - } - else - std::cout << std::endl << "Failed to write the file " << filepath << std::endl; - - std::cout << " ========= END ===========" << std::endl << std::endl; - //ceres_manager->removeConstraints(); - delete wolf_manager; - std::cout << "everything deleted!\n"; - delete ceres_manager; - std::cout << "...deleted!\n"; - delete ceres_problem; - std::cout << "amost... deleted!\n"; - - //exit - return 0; -} - diff --git a/src/examples/test_ceres_manager_tree.cpp b/src/examples/test_ceres_manager_tree.cpp deleted file mode 100644 index 9773203764a742873075e2c515b0566915595647..0000000000000000000000000000000000000000 --- a/src/examples/test_ceres_manager_tree.cpp +++ /dev/null @@ -1,360 +0,0 @@ -//std includes -#include <cstdlib> -#include <iostream> -#include <fstream> -#include <memory> -#include <random> -#include <typeinfo> -#include <ctime> -#include <queue> - -// Eigen includes -#include <eigen3/Eigen/Dense> -#include <eigen3/Eigen/Geometry> - -//Ceres includes -#include "ceres/ceres.h" -#include "glog/logging.h" - -//Wolf includes -#include "wolf.h" -#include "sensor_base.h" -#include "sensor_odom_2D.h" -#include "sensor_gps_fix.h" -#include "frame_base.h" -#include "state_point.h" -#include "state_complex_angle.h" -#include "capture_base.h" -#include "capture_relative.h" -#include "capture_odom_2D.h" -#include "capture_gps_fix.h" -#include "state_base.h" -#include "constraint_sparse.h" -#include "constraint_gps_2D.h" -#include "constraint_odom_2D_theta.h" -#include "constraint_odom_2D_complex_angle.h" - -// ceres wrapper include -#include "ceres_wrapper/ceres_manager.h" - -/** - * This test implements an optimization using CERES of a vehicle trajectory using odometry and GPS simulated data. - * - **/ - -using namespace Eigen; - -class WolfManager -{ - protected: - VectorXs state_; - unsigned int first_empty_state_; - bool use_complex_angles_; - TrajectoryBase* trajectory_; - std::vector<VectorXs> odom_captures_; - std::vector<VectorXs> gps_captures_; - std::queue<CaptureBase*> new_captures_; - SensorBase* sensor_prior_; - - public: - WolfManager(SensorBase* _sensor_prior, const bool _complex_angle, const unsigned int& _state_length=1000) : - state_(_state_length), - first_empty_state_(0), - use_complex_angles_(_complex_angle), - trajectory_(new TrajectoryBase()), - sensor_prior_(_sensor_prior) - { - VectorXs init_frame(use_complex_angles_ ? 4 : 3); - if (use_complex_angles_) - init_frame << 0, 0, 1, 0; - else - init_frame << 0, 0, 0; - createFrame(init_frame, 0); - } - - virtual ~WolfManager() - { - delete trajectory_; - } - - void createFrame(const VectorXs& _frame_state, const TimeStamp& _time_stamp) - { - // Store in state_ - state_.segment(first_empty_state_, use_complex_angles_ ? 4 : 3) << _frame_state; - - // Create frame and add it to the trajectory - if (use_complex_angles_) - { - FrameBase* new_frame = new FrameBase(_time_stamp, - new StateBlock(state_.data()+first_empty_state_), - new StateComplexAngle(state_.data()+first_empty_state_+2)); - trajectory_->addFrame(new_frame); - } - else - { - FrameBase* new_frame = new FrameBase(_time_stamp, - new StateBlock(state_.data()+first_empty_state_), - new StateBlock(state_.data()+first_empty_state_+2)); - trajectory_->addFrame(new_frame); - } - - // Update first free state location index - first_empty_state_ += use_complex_angles_ ? 4 : 3; - } - - void addCapture(CaptureBase* _capture) - { - new_captures_.push(_capture); - } - - void update(std::list<StateBlock*>& new_state_units, std::list<ConstraintBase*>& new_constraints) - { - // TODO: management due to time stamps - while (!new_captures_.empty()) - { - // EXTRACT NEW CAPTURE - CaptureBase* new_capture = new_captures_.front(); - new_captures_.pop(); - - // NEW FRAME (if the specific sensor) - // TODO: accumulate odometries - if (new_capture->getSensorPtr() == sensor_prior_) - { - // ADD CAPTURE TO THE PREVIOUS FRAME - trajectory_->getFrameListPtr()->back()->addCapture(new_capture); - - // COMPUTE PRIOR - trajectory_->getFrameListPtr()->back()->setState(new_capture->computePrior()); - - // CREATE NEW FRAME - createFrame(VectorXs::Zero(use_complex_angles_ ? 4 : 3), new_capture->getTimeStamp()); - - // NEW STATE UNITS TO BE ADDED BY CERES - //new_state_units.insert(new_state_units.end(), trajectory_.getFrameList.back()->getStateList().begin(), trajectory_.getFrameList.back()->getStateList().end()); - new_state_units.push_back(trajectory_->getFrameListPtr()->back()->getPPtr()); - new_state_units.push_back(trajectory_->getFrameListPtr()->back()->getOPtr()); - } - else - { - // ADD CAPTURE TO THE LAST FRAME - trajectory_->getFrameListPtr()->back()->addCapture(new_capture); - } - - // COMPUTE CAPTURE (features, constraints) - new_capture->processCapture(); - - // ADD CORRESPONDENCES TO THE new_constraints OUTPUT PARAM - for (FeatureBaseIter feature_list_iter=new_capture->getFeatureListPtr()->begin(); feature_list_iter!=new_capture->getFeatureListPtr()->end(); feature_list_iter++) - { - for (ConstraintBaseIter constraint_list_iter=(*feature_list_iter)->getConstraintListPtr()->begin(); constraint_list_iter!=(*feature_list_iter)->getConstraintListPtr()->end(); constraint_list_iter++) - { - new_constraints.push_back(*constraint_list_iter); - } - } - } - } - - VectorXs getState() - { - return state_; - } - - std::list<StateBlock*> getStateList() - { - std::list<StateBlock*> st_list; - - for (FrameBaseIter frame_list_iter=trajectory_->getFrameListPtr()->begin(); frame_list_iter!=trajectory_->getFrameListPtr()->end(); frame_list_iter++) - { - //st_list.insert(st_list.end(), (*frame_list_iter)->getStateList().begin(), (*frame_list_iter)->getStateList().end()); - st_list.push_back((*frame_list_iter)->getPPtr()); - st_list.push_back((*frame_list_iter)->getOPtr()); - } - - return st_list; - } - - std::list<ConstraintBase*> getConstraintsList() - { - std::list<ConstraintBase*> corr_list; - - for (FrameBaseIter frame_list_iter=trajectory_->getFrameListPtr()->begin(); frame_list_iter!=trajectory_->getFrameListPtr()->end(); frame_list_iter++) - { - for (CaptureBaseIter capture_list_iter=(*frame_list_iter)->getCaptureListPtr()->begin(); capture_list_iter!=(*frame_list_iter)->getCaptureListPtr()->end(); capture_list_iter++) - { - for (FeatureBaseIter feature_list_iter=(*capture_list_iter)->getFeatureListPtr()->begin(); feature_list_iter!=(*capture_list_iter)->getFeatureListPtr()->end(); feature_list_iter++) - { - corr_list.insert(corr_list.end(),(*feature_list_iter)->getConstraintListPtr()->begin(), (*feature_list_iter)->getConstraintListPtr()->end()); - } - } - } - return corr_list; - } - - void printTree() - { - trajectory_->print(); - } -}; - -int main(int argc, char** argv) -{ - std::cout << "\n ========= 2D Robot with odometry and GPS ===========\n"; - - // USER INPUT ============================================================================================ - if (argc!=3 || atoi(argv[1])<1 || atoi(argv[2]) < 0 || atoi(argv[2]) > 1) - { - std::cout << "Please call me with: [./test_ceres_manager NI PRINT ORIENTATION_MODE], where:" << std::endl; - std::cout << " - NI is the number of iterations (NI > 0)" << std::endl; - std::cout << " - ORIENTATION_MODE: 0 for theta, 1 for complex angle" << std::endl; - std::cout << "EXIT due to bad user input" << std::endl << std::endl; - return -1; - } - - clock_t t1, t2; - t1=clock(); - - unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution - bool complex_angle = (bool) atoi(argv[2]); - - // INITIALIZATION ============================================================================================ - //init random generators - WolfScalar odom_std= 0.01; - WolfScalar gps_std= 1; - std::default_random_engine generator(1); - std::normal_distribution<WolfScalar> distribution_odom(0.001, odom_std); //odometry noise - std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise - - //init google log - google::InitGoogleLogging(argv[0]); - - // Ceres initialization - ceres::Solver::Options ceres_options; - ceres_options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION; - ceres_options.max_line_search_step_contraction = 1e-3; - // ceres_options.minimizer_progress_to_stdout = false; - // ceres_options.line_search_direction_type = ceres::LBFGS; - // ceres_options.max_num_iterations = 100; - ceres::Problem::Options problem_options; - problem_options.cost_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - problem_options.loss_function_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP; - CeresManager* ceres_manager = new CeresManager(problem_options); - std::ofstream log_file; //output file - - - //variables - Eigen::VectorXs odom_inc_true(n_execution*2);//invented motion - Eigen::VectorXs pose_true(3); //current true pose - Eigen::VectorXs pose_odom(3); //current true pose - Eigen::VectorXs ground_truth(n_execution*3); //all true poses - Eigen::VectorXs odom_trajectory(n_execution*3); //all true poses - Eigen::VectorXs odom_readings(n_execution*2); // all odometry readings - Eigen::VectorXs gps_fix_readings(n_execution*3); //all GPS fix readings - std::list<StateBlock*> new_state_units; // new state units in wolf that must be added to ceres - std::list<ConstraintBase*> new_constraints; // new constraints in wolf that must be added to ceres - - // Wolf manager initialization - SensorOdom2D odom_sensor(Eigen::MatrixXs::Zero(6,1), odom_std, odom_std); - SensorGPSFix gps_sensor(Eigen::MatrixXs::Zero(6,1), gps_std); - WolfManager* wolf_manager = new WolfManager(&odom_sensor, complex_angle, n_execution * (complex_angle ? 4 : 3)); - - // Initial pose - pose_true << 0,0,0; - pose_odom << 0,0,0; - ground_truth.head(3) = pose_true; - odom_trajectory.head(3) = pose_true; - - // SENSOR DATA ============================================================================================ - for (unsigned int ii = 1; ii<n_execution; ii++) - { - // inventing odometry ground truth - if ( ii < (unsigned int)floor(n_execution/2) ) - odom_inc_true.segment(ii*2,2) << fabs(cos(ii/10.)) , fabs(sin(ii/2000.)); //invented motion increments. - else - odom_inc_true.segment(ii*2,2) << fabs(cos(ii/10.)) , -fabs(sin((ii-floor(n_execution/2))/2000.)); //invented motion increments. - - // Computing ground truth trajectory - pose_true(0) = pose_true(0) + odom_inc_true(ii*2) * cos(pose_true(2) + odom_inc_true(ii*2+1)); - pose_true(1) = pose_true(1) + odom_inc_true(ii*2) * sin(pose_true(2) + odom_inc_true(ii*2+1)); - pose_true(2) = pose_true(2) + odom_inc_true(ii*2+1); - ground_truth.segment(ii*3,3) << pose_true; - - // corrupting sensor readings (odometry and GPS) - odom_readings.segment(ii*2,2) << odom_inc_true(ii*2) + distribution_odom(generator), - odom_inc_true(ii*2+1) + distribution_odom(generator); //true range and theta with noise - gps_fix_readings.segment(ii*3,3) << pose_true(0) + distribution_gps(generator), - pose_true(1) + distribution_gps(generator), - 0. + distribution_gps(generator); - - // Computing ground truth trajectory - pose_odom(0) = pose_odom(0) + odom_readings(ii*2) * cos(pose_odom(2) + odom_readings(ii*2+1)); - pose_odom(1) = pose_odom(1) + odom_readings(ii*2) * sin(pose_odom(2) + odom_readings(ii*2+1)); - pose_odom(2) = pose_odom(2) + odom_readings(ii*2+1); - odom_trajectory.segment(ii*3,3) << pose_odom; - } - - // START TRAJECTORY ============================================================================================ - new_state_units = wolf_manager->getStateList(); // First pose to be added in ceres - for (unsigned int step=1; step < n_execution; step++) - { - // adding new sensor captures - wolf_manager->addCapture(new CaptureOdom2D(TimeStamp(step*0.01), &odom_sensor, odom_readings.segment(step*2,2), odom_std * MatrixXs::Identity(2,2))); - wolf_manager->addCapture(new CaptureGPSFix(TimeStamp(step*0.01), &gps_sensor, gps_fix_readings.segment(step*3,3), gps_std * MatrixXs::Identity(3,3))); - - // updating problem - wolf_manager->update(new_state_units, new_constraints); - - // adding new state units and constraints to ceres - ceres_manager->addStateUnits(&new_state_units); - ceres_manager->addConstraints(&new_constraints); - } - - //std::cout << "Resulting tree:\n"; - //wolf_manager->printTree(); - - // SOLVE OPTIMIZATION ============================================================================================ - ceres::Solver::Summary summary = ceres_manager->solve(ceres_options); - t2=clock(); - double seconds = ((double)t2-t1)/CLOCKS_PER_SEC; - - // DISPLAY RESULTS ============================================================================================ - std::cout << summary.FullReport() << std::endl; - std::cout << "optimization seconds: " << summary.total_time_in_seconds << std::endl; - std::cout << "total seconds: " << seconds << std::endl; - - // change from complex angle to theta - VectorXs state = wolf_manager->getState(); - VectorXs state_theta(n_execution * 3); - if (complex_angle) - for (unsigned int ii = 0; ii<n_execution; ii++) - state_theta.segment(ii*3,3) << state(ii*4), state(ii*4+1), atan2(state(ii*4+2), state(ii*4+3)); - else - state_theta = state; - - // Print log file - std::string filepath = getenv("HOME") + (complex_angle ? std::string("/Desktop/log_file_3.txt") : std::string("/Desktop/log_file_2.txt")); - log_file.open(filepath, std::ofstream::out); //open log file - - if (log_file.is_open()) - { - log_file << seconds << std::endl; - for (unsigned int ii = 0; ii<n_execution; ii++) - log_file << state_theta.segment(ii*3,3).transpose() - << "\t" << ground_truth.segment(ii*3,3).transpose() - << "\t" << (state_theta.segment(ii*3,3)-ground_truth.segment(ii*3,3)).transpose() - << "\t" << odom_trajectory.segment(ii*3,3).transpose() - << "\t" << gps_fix_readings.segment(ii*3,3).transpose() << std::endl; - log_file.close(); //close log file - std::cout << std::endl << "Result file " << filepath << std::endl; - } - else - std::cout << std::endl << "Failed to write the file " << filepath << std::endl; - - std::cout << " ========= END ===========" << std::endl << std::endl; - - delete ceres_manager; - delete wolf_manager; - - //exit - return 0; -} diff --git a/src/examples/test_ceres_odom_batch.cpp b/src/examples/test_ceres_odom_batch.cpp deleted file mode 100644 index 0178bd5c10f9097eca906d52bffdcce773897c13..0000000000000000000000000000000000000000 --- a/src/examples/test_ceres_odom_batch.cpp +++ /dev/null @@ -1,450 +0,0 @@ -// Testing a full wolf tree avoiding template classes for NodeLinked derived classes - -//std includes -#include <ctime> -#include <cstdlib> -#include <iostream> -#include <fstream> -#include <vector> -#include <list> -#include <random> -#include <cmath> -// #include <memory> -// #include <typeinfo> - -//ceres -#include "ceres/ceres.h" - -//Wolf includes -#include "wolf.h" -#include "time_stamp.h" -#include "node_terminus.h" -#include "node_linked.h" - -//forward declarations -class TrajectoryBaseX; -class FrameBaseX; -class CaptureBaseX; -class FeatureBaseX; -class CorrespondenceBaseX; - -//class TrajectoryBaseX -class TrajectoryBaseX : public NodeLinked<NodeTerminus,FrameBaseX> -{ - protected: - unsigned int length_; //just something to play - - public: - TrajectoryBaseX(const unsigned int _len) : - NodeLinked(TOP, "TRAJECTORY"), - length_(_len) - { - // - }; - - virtual ~TrajectoryBaseX() - { - - }; -}; - -//class FrameBaseX -class FrameBaseX : public NodeLinked<TrajectoryBaseX,CaptureBaseX> -{ - protected: - double time_stamp_; //frame ts - - public: - FrameBaseX(double _ts) : - NodeLinked(MID, "FRAME"), - time_stamp_(_ts) - { - // - }; - - virtual ~FrameBaseX() - { - - }; -}; - -//class CaptureBaseX -class CaptureBaseX : public NodeLinked<FrameBaseX,FeatureBaseX> -{ - protected: - double time_stamp_; //capture ts - - public: - CaptureBaseX(double _ts) : - NodeLinked(MID, "CAPTURE"), - time_stamp_(_ts) - { - // - }; - - virtual ~CaptureBaseX() - { - - }; -}; - -//class FeatureBaseX -class FeatureBaseX : public NodeLinked<CaptureBaseX,CorrespondenceBaseX> -{ - protected: - - public: - FeatureBaseX() : - NodeLinked(MID, "FEATURE") - { - // - }; - - virtual ~FeatureBaseX() - { - // - }; -}; - -//class CorrespondenceBaseX -class CorrespondenceBaseX : public NodeLinked<FeatureBaseX,NodeTerminus> -{ - protected: - unsigned int nblocks_; //number of state blocks in which the correspondence depends on. - std::vector<unsigned int> block_indexes_; //state vector indexes indicating start of each state block. This vector has nblocks_ size. - std::vector<unsigned int> block_sizes_; //sizes of each state block. This vector has nblocks_ size. - ceres::CostFunction* cost_function_ptr_; - - public: - CorrespondenceBaseX(const unsigned int _nb, const std::vector<unsigned int> & _bindexes, const std::vector<unsigned int> & _bsizes) : - NodeLinked(BOTTOM, "CORRESPONDENCE"), - nblocks_(_nb), - block_indexes_(_bindexes), - block_sizes_(_bsizes) - { - assert(block_sizes_.size() == nblocks_); - }; - - virtual ~CorrespondenceBaseX() - { - // - }; - - ceres::CostFunction * getCostFunctionPtr() - { - return cost_function_ptr_; - }; - - virtual void display() const - { - unsigned int ii; - std::cout << "number of blocks: " << nblocks_ << std::endl; - std::cout << "block indexes: "; - for (ii=0; ii<block_indexes_.size(); ii++) std::cout << block_indexes_.at(ii) << " "; - std::cout << std::endl; - std::cout << "block sizes: "; - for (ii=0; ii<block_sizes_.size(); ii++) std::cout << block_sizes_.at(ii) << " "; - std::cout << std::endl; - }; -}; - -class Odom2DFunctor -{ - protected: - Eigen::Vector2s odom_inc_; //incremental odometry measurements (range, theta). Could be a map to data hold by capture or feature - const double odom_stdev_ = 0.01; //model parameters - - public: - //constructor - Odom2DFunctor(const Eigen::Vector2s & _odom): - odom_inc_(_odom) - { - // - }; - - //destructor - virtual ~Odom2DFunctor() - { - // - }; - - //cost function - template <typename T> - bool operator()(const T* const _x0, const T* const _x1, T* _residual) const - { - T dr2, dth; - - //expected range and theta increments, given the state points - dr2 = (_x0[0]-_x1[0])*(_x0[0]-_x1[0]) + (_x0[1]-_x1[1])*(_x0[1]-_x1[1]); //square of the range - dth = _x1[2] - _x0[2]; // - - //residuals in range and theta components - _residual[0] = (dr2 - T(odom_inc_(0)*odom_inc_(0))) / T(odom_stdev_); - _residual[1] = (dth - T(odom_inc_(1))) / T(odom_stdev_); - - //return - return true; - }; -}; - -//Specialized correspondence class for 2D odometry -class CorrespondenceOdom2D : public CorrespondenceBaseX -{ - protected: - Eigen::Map<Eigen::Vector3s> pose_previous_; - Eigen::Map<Eigen::Vector3s> pose_current_; - Eigen::Map<const Eigen::Vector2s> odom_inc_; - - public: - CorrespondenceOdom2D(WolfScalar * _st, const Eigen::Vector2s & _odom) : - CorrespondenceBaseX(2,{0,3},{3,3}), - pose_previous_(_st + block_indexes_.at(0), block_sizes_.at(0)), - pose_current_(_st + block_indexes_.at(1), block_sizes_.at(1)), - odom_inc_(_odom.data()) - { - cost_function_ptr_ = new ceres::AutoDiffCostFunction<Odom2DFunctor,2,3,3>(new Odom2DFunctor(_odom)); - }; - - virtual ~CorrespondenceOdom2D() - { - //delete cost_function_ptr_; - }; - - double * getPosePreviousPtr() - { - return pose_previous_.data(); - }; - - double * getPoseCurrentPtr() - { - return pose_current_.data(); - }; - - virtual void display() const - { - std::cout << "---- Odom Correspondence ----" << std::endl; - CorrespondenceBaseX::display(); - std::cout << "pose_previous_: " << pose_previous_.transpose() << std::endl; - std::cout << "pose_current_: " << pose_current_.transpose() << std::endl; - std::cout << "odom_inc_: " << odom_inc_.transpose() << std::endl; - }; -}; - -class GPSFixFunctor -{ - protected: - Eigen::Vector3s gps_fix_; //GPS fix XYZ. Could be a map to data hold by capture or feature - const double gps_stdev_ = 1; //model parameters - - public: - //constructor - GPSFixFunctor(const Eigen::Vector3s & _gps_fix): - gps_fix_(_gps_fix) - { - // - }; - - //destructor - virtual ~GPSFixFunctor() - { - // - }; - - //cost function - template <typename T> - bool operator()(const T* const _x0, T* _residual) const - { - T dist_x = T( gps_fix_(0) ) - _x0[0]; - T dist_y = T( gps_fix_(1) ) - _x0[1]; - _residual[0] = dist_x / T(gps_stdev_); - _residual[1] = dist_y / T(gps_stdev_); - _residual[2] = T(0.); //T( gps_fix_(2) ) - _x0[2]; //in this case, third component of the state is heading, not Z, so it is ignored - return true; - }; -}; - -//Specialized correspondence class for GPS Fix data -class CorrespondenceGPSFix : public CorrespondenceBaseX -{ - protected: - Eigen::Map<Eigen::Vector3s> location_; - Eigen::Map<const Eigen::Vector3s> gps_fix_; - //TODO: add ceres_residual_block_id_ to manage add/remove block - - public: - CorrespondenceGPSFix(WolfScalar * _st, const Eigen::Vector3s & _gps_fix) : - CorrespondenceBaseX(1,{0},{3}), - location_(_st + block_indexes_.at(0) , block_sizes_.at(0)), - gps_fix_(_gps_fix.data()) - { - cost_function_ptr_ = new ceres::AutoDiffCostFunction<GPSFixFunctor,3,3>(new GPSFixFunctor(_gps_fix)); - }; - - virtual ~CorrespondenceGPSFix() - { - //delete cost_function_ptr_; - }; - - double * getLocation() - { - return location_.data(); - } - - virtual void display() const - { - std::cout << "---- GPS Fix Correspondence ----" << std::endl; - CorrespondenceBaseX::display(); - std::cout << "location_: " << location_.transpose() << std::endl; - std::cout << "gps_fix_: " << gps_fix_.transpose() << std::endl; - }; -}; - - -int main(int argc, char** argv) -{ - clock_t t1, t2; - t1 = clock(); - - //Welcome message - std::cout << std::endl << " ========= WOLF-CERES test. Simple Odometry + GPS fix problem (with non-template classes) ===========" << std::endl << std::endl; - - //user input - if (argc!=2) - { - std::cout << "Please call me with: [./test_ceres_wrapper_non_template NI NW], where:" << std::endl; - std::cout << " - NI is the number of iterations" << std::endl; - std::cout << "EXIT due to bad user input" << std::endl << std::endl; - return -1; - } - unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution - - //init google log - google::InitGoogleLogging(argv[0]); - - //variables - std::list<unsigned int> block_ids; //id of each added block to ceres problem - Eigen::VectorXs odom_inc_true;//invented motion - Eigen::Vector3s pose_true; //current true pose - Eigen::VectorXs ground_truth; //accumulated true poses - Eigen::Vector3s pose_predicted; // current predicted pose - Eigen::VectorXs predicted_trajectory; // current predicted pose - Eigen::VectorXs state; //running window winth solver result - Eigen::Vector2s odom_reading; //current odometry reading - Eigen::Vector3s gps_fix_reading; //current GPS fix reading - Eigen::VectorXs gps_log; //log of all gps readings - CorrespondenceOdom2D *odom_corresp; //pointer to odometry correspondence - CorrespondenceGPSFix *gps_fix_corresp; //pointer to GPS fix correspondence - ceres::Problem problem; //ceres problem - ceres::Solver::Options options; //ceres solver options - ceres::Solver::Summary summary; //ceres solver summary - std::ofstream log_file; //output file - - //resize vectors according user input number of iterations - odom_inc_true.resize(n_execution*2); //2 odometry components per iteration - ground_truth.resize(n_execution*3);// 3 components per iteration - gps_log.resize(n_execution*3); //3 components per iteration - state.resize(n_execution*3); //3 components per window element - predicted_trajectory.resize(n_execution*3); //3 components per window element - - //init true odom and true pose - for (unsigned int ii = 0; ii<n_execution; ii++) - { - if ( ii < (unsigned int)floor(n_execution/2) ) - odom_inc_true.middleRows(ii*2,2) << fabs(cos(ii/10.)) , fabs(sin(ii/2000.)); //invented motion increments. - else - odom_inc_true.middleRows(ii*2,2) << fabs(cos(ii/10.)) , -fabs(sin((ii-floor(n_execution/2))/2000.)); //invented motion increments. - } - pose_true << 0,0,0; - pose_predicted << 0,0,0; - ground_truth.middleRows(0,3) << pose_true; //init point pushed to ground truth - state.middleRows(0,3) << 0,0,0; //init state at origin - predicted_trajectory.middleRows(0,3) << pose_predicted; - - //init random generators - std::default_random_engine generator(1); - std::normal_distribution<WolfScalar> distribution_odom(0.001,0.01); //odometry noise - std::normal_distribution<WolfScalar> distribution_gps(0.0,1); //GPS noise - - //test loop - for (unsigned int ii = 1; ii<n_execution; ii++) //ii over iterations - { - //inventing a simple motion - pose_true(0) = pose_true(0) + odom_inc_true(ii*2) * cos(pose_true(2)+odom_inc_true(ii*2+1)); - pose_true(1) = pose_true(1) + odom_inc_true(ii*2) * sin(pose_true(2)+odom_inc_true(ii*2+1)); - pose_true(2) = pose_true(2) + odom_inc_true(ii*2+1); - ground_truth.middleRows(ii*3,3) << pose_true; - //std::cout << "pose_true(" << ii << ") = " << pose_true.transpose() << std::endl; - - //inventing sensor readings for odometry and GPS - odom_reading << odom_inc_true(ii*2)+distribution_odom(generator), odom_inc_true(ii*2+1)+distribution_odom(generator); //true range and theta with noise - gps_fix_reading << pose_true(0) + distribution_gps(generator), pose_true(1) + distribution_gps(generator), 0. + distribution_gps(generator); - gps_log.middleRows(ii*3,3) << gps_fix_reading;//log the reading - - //setting initial guess as an odometry prediction, using noisy odometry - pose_predicted(0) = pose_predicted(0) + odom_reading(0) * cos(pose_predicted(2)+odom_reading(1)); - pose_predicted(1) = pose_predicted(1) + odom_reading(0) * sin(pose_predicted(2)+odom_reading(1)); - pose_predicted(2) = pose_predicted(2) + odom_reading(1); - state.middleRows(ii*3,3) << pose_predicted; - predicted_trajectory.middleRows(ii*3,3) << pose_predicted; - //std::cout << "pose_predicted(" << ii << ") = " << pose_predicted.transpose() << std::endl; - - //creating odom correspondence, exceptuating first iteration. Adding it to the problem - odom_corresp = new CorrespondenceOdom2D(state.data()+(ii-1)*3, odom_reading); - //odom_corresp->display(); - problem.AddResidualBlock(odom_corresp->getCostFunctionPtr(),nullptr, odom_corresp->getPosePreviousPtr(), odom_corresp->getPoseCurrentPtr()); - delete odom_corresp; - - //creating gps correspondence and adding it to the problem - gps_fix_corresp = new CorrespondenceGPSFix(state.data()+ii*3, gps_fix_reading); - //gps_fix_corresp->display(); - problem.AddResidualBlock(gps_fix_corresp->getCostFunctionPtr(),nullptr, gps_fix_corresp->getLocation()); - delete gps_fix_corresp; - - //set options and solve (sliding window) -// options.minimizer_progress_to_stdout = true; -// ceres::Solve(options, &problem, &summary); - } - - //display initial guess - //std::cout << "INITIAL GUESS IS: " << state.transpose() << std::endl; - - //set options and solve (batch mode) - //options.minimizer_progress_to_stdout = true; - options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION;// - options.max_line_search_step_contraction = 1e-3; - ceres::Solve(options, &problem, &summary); - - t2=clock(); - double seconds = ((double)t2-t1)/CLOCKS_PER_SEC; - std::cout << summary.FullReport() << std::endl; - std::cout << "optimization seconds: " << summary.total_time_in_seconds << std::endl; - std::cout << "total seconds: " << seconds << std::endl; - - //display/log results, by setting cout flags properly - std::string homepath = getenv("HOME"); - log_file.open(homepath + "/Desktop/log_file.txt", std::ofstream::out); //open log file - if (log_file.is_open()) - { - log_file << seconds << std::endl; - for (unsigned int ii = 0; ii<n_execution; ii++) - log_file << state.middleRows(ii*3,3).transpose() - << " " << ground_truth.middleRows(ii*3,3).transpose() - << " " << (state.middleRows(ii*3,3)-ground_truth.middleRows(ii*3,3)).transpose() - << " " << gps_log.middleRows(ii*3,3).transpose() - << " " << predicted_trajectory.middleRows(ii*3,3).transpose() << std::endl; - log_file.close(); //close log file - std::cout << std::endl << " Result to file ~/Desktop/log_data.txt" << std::endl; - } - else - std::cout << std::endl << " Failed to write the file ~/Desktop/log_data.txt" << std::endl; - - //free memory (not necessary since ceres::problem holds their ownership) -// delete odom_corresp; -// delete gps_fix_corresp; - - //End message - std::cout << " =========================== END ===============================" << std::endl << std::endl; - - //exit - return 0; -} diff --git a/src/examples/test_ceres_odom_iterative.cpp b/src/examples/test_ceres_odom_iterative.cpp deleted file mode 100644 index 91e7550827db7079781f8a6a586aea0bfc11e6f3..0000000000000000000000000000000000000000 --- a/src/examples/test_ceres_odom_iterative.cpp +++ /dev/null @@ -1,459 +0,0 @@ -// Testing a full wolf tree avoiding template classes for NodeLinked derived classes - -//std includes -#include <cstdlib> -#include <stdlib.h> //getenv -#include <iostream> -#include <fstream> -#include <string> -#include <vector> -#include <list> -#include <random> -#include <cmath> -// #include <memory> -// #include <typeinfo> - -//ceres -#include "ceres/ceres.h" - -//Wolf includes -#include "wolf.h" -#include "node_terminus.h" -#include "node_linked.h" - -//forward declarations -class TrajectoryBaseX; -class FrameBaseX; -class CaptureBaseX; -class FeatureBaseX; -class CorrespondenceBaseX; - -//class TrajectoryBaseX -class TrajectoryBaseX : public NodeLinked<NodeTerminus,FrameBaseX> -{ - protected: - unsigned int length_; //just something to play - - public: - TrajectoryBaseX(const unsigned int _len) : - NodeLinked(TOP, "TRAJECTORY"), - length_(_len) - { - // - }; - - virtual ~TrajectoryBaseX() - { - - }; -}; - -//class FrameBaseX -class FrameBaseX : public NodeLinked<TrajectoryBaseX,CaptureBaseX> -{ - protected: - double time_stamp_; //frame ts - - public: - FrameBaseX(double _ts) : - NodeLinked(MID, "FRAME"), - time_stamp_(_ts) - { - // - }; - - virtual ~FrameBaseX() - { - - }; -}; - -//class CaptureBaseX -class CaptureBaseX : public NodeLinked<FrameBaseX,FeatureBaseX> -{ - protected: - double time_stamp_; //capture ts - - public: - CaptureBaseX(double _ts) : - NodeLinked(MID, "CAPTURE"), - time_stamp_(_ts) - { - // - }; - - virtual ~CaptureBaseX() - { - - }; -}; - -//class FeatureBaseX -class FeatureBaseX : public NodeLinked<CaptureBaseX,CorrespondenceBaseX> -{ - protected: - - public: - FeatureBaseX() : - NodeLinked(MID, "FEATURE") - { - // - }; - - virtual ~FeatureBaseX() - { - // - }; -}; - -//class CorrespondenceBaseX -class CorrespondenceBaseX : public NodeLinked<FeatureBaseX,NodeTerminus> -{ - protected: - unsigned int nblocks_; //number of state blocks in which the correspondence depends on. - //std::vector<unsigned int> block_indexes_; //state vector indexes indicating start of each state block. This vector has nblocks_ size. - std::vector<unsigned int> block_sizes_; //sizes of each state block. This vector has nblocks_ size. - ceres::CostFunction* cost_function_ptr_; - ceres::ResidualBlockId ceres_residual_block_id_; - - public: - //CorrespondenceBaseX(const unsigned int _nb, const std::vector<unsigned int> & _bindexes, const std::vector<unsigned int> & _bsizes) : - CorrespondenceBaseX(const unsigned int _nb, const std::vector<unsigned int> & _bsizes) : - NodeLinked(BOTTOM, "CORRESPONDENCE"), - nblocks_(_nb), - //block_indexes_(_bindexes), - block_sizes_(_bsizes) - { - assert(block_sizes_.size() == nblocks_); - }; - - virtual ~CorrespondenceBaseX() - { - // - }; - -// ceres::CostFunction * getCostFunctionPtr() -// { -// return cost_function_ptr_; -// }; - - virtual void addToProblem(ceres::Problem & _problem) = 0; - - virtual void removeFromProblem(ceres::Problem & _problem) - { - _problem.RemoveResidualBlock(ceres_residual_block_id_); - } - - virtual void display() const - { - unsigned int ii; - std::cout << "number of state blocks: " << nblocks_ << std::endl; - //std::cout << "state block indexes: "; - //for (ii=0; ii<block_indexes_.size(); ii++) std::cout << block_indexes_.at(ii) << " "; - std::cout << std::endl; - std::cout << "state block sizes: "; - for (ii=0; ii<block_sizes_.size(); ii++) std::cout << block_sizes_.at(ii) << " "; - std::cout << std::endl; - std::cout << "ceres residual block id: " << ceres_residual_block_id_ << std::endl; - }; -}; - -class Odom2DFunctor -{ - protected: - Eigen::Vector2s odom_inc_; //incremental odometry measurements (range, theta). Could be a map to data hold by capture or feature - const double odom_stdev_ = 0.01; //model parameters - - public: - //constructor - Odom2DFunctor(const Eigen::Vector2s & _odom): - odom_inc_(_odom) - { - // - }; - - //destructor - virtual ~Odom2DFunctor() - { - // - }; - - //cost function - template <typename T> - bool operator()(const T* const _x0, const T* const _x1, T* _residual) const - { - T dr2, dth; - - //expected range and theta increments, given the state points - dr2 = (_x0[0]-_x1[0])*(_x0[0]-_x1[0]) + (_x0[1]-_x1[1])*(_x0[1]-_x1[1]); //square of the range - dth = _x1[2] - _x0[2]; // - - //residuals in range and theta components - _residual[0] = (T(dr2) - T(odom_inc_(0)*odom_inc_(0))) / T(odom_stdev_); - _residual[1] = (T(dth) - T(odom_inc_(1))) / T(odom_stdev_); - - //return - return true; - }; -}; - -//Specialized correspondence class for 2D odometry -class CorrespondenceOdom2D : public CorrespondenceBaseX -{ - protected: - Eigen::Map<Eigen::Vector3s> pose_previous_; - Eigen::Map<Eigen::Vector3s> pose_current_; - Eigen::Map<const Eigen::Vector2s> odom_inc_; - - public: - CorrespondenceOdom2D(WolfScalar * _st_prev, WolfScalar * _st_curr, const Eigen::Vector2s & _odom) : - CorrespondenceBaseX(2,{3,3}), - pose_previous_(_st_prev),//, block_sizes_.at(0)), //size 3 is already defined at declaration - pose_current_(_st_curr),//, block_sizes_.at(1)), //size 3 is already defined at declaration - odom_inc_(_odom.data()) - { - cost_function_ptr_ = new ceres::AutoDiffCostFunction<Odom2DFunctor,2,3,3>(new Odom2DFunctor(_odom)); - }; - - virtual ~CorrespondenceOdom2D() - { - //delete cost_function_ptr_; - }; - -// double * getPosePreviousPtr() -// { -// return pose_previous_.data(); -// }; -// -// double * getPoseCurrentPtr() -// { -// return pose_current_.data(); -// }; - - virtual void addToProblem(ceres::Problem & _problem) - { - ceres_residual_block_id_ = _problem.AddResidualBlock(cost_function_ptr_,nullptr, pose_previous_.data(), pose_current_.data()); - } - - virtual void display() const - { - std::cout << "---- Odom Correspondence ----" << std::endl; - CorrespondenceBaseX::display(); - std::cout << "pose_previous_: " << pose_previous_.transpose() << std::endl; - std::cout << "pose_current_: " << pose_current_.transpose() << std::endl; - std::cout << "odom_inc_: " << odom_inc_.transpose() << std::endl; - }; -}; - -class GPSFixFunctor -{ - protected: - Eigen::Vector3s gps_fix_; //GPS fix XYZ. Could be a map to data hold by capture or feature - const double gps_stdev_ = 1; //model parameters - - public: - //constructor - GPSFixFunctor(const Eigen::Vector3s & _gps_fix): - gps_fix_(_gps_fix) - { - // - }; - - //destructor - virtual ~GPSFixFunctor() - { - // - }; - - //cost function - template <typename T> - bool operator()(const T* const _x0, T* _residual) const - { - T dist_x = T( gps_fix_(0) ) - _x0[0]; - T dist_y = T( gps_fix_(1) ) - _x0[1]; - _residual[0] = dist_x / T(gps_stdev_); - _residual[1] = dist_y / T(gps_stdev_); - _residual[2] = T(0.); //T( gps_fix_(2) ) - _x0[2]; //in this case, third component of the state is heading, not Z, so it is ignored - return true; - }; -}; - -//Specialized correspondence class for GPS Fix data -class CorrespondenceGPSFix : public CorrespondenceBaseX -{ - protected: - Eigen::Map<Eigen::Vector3s> state_position_; - Eigen::Map<const Eigen::Vector3s> gps_fix_; - - public: - CorrespondenceGPSFix(WolfScalar * _st, const Eigen::Vector3s & _gps_fix) : - CorrespondenceBaseX(1,{3}), - state_position_(_st),// block_sizes_.at(0)), //size 3 is already defined at declaration - gps_fix_(_gps_fix.data()) - { - cost_function_ptr_ = new ceres::AutoDiffCostFunction<GPSFixFunctor,3,3>(new GPSFixFunctor(_gps_fix)); - }; - - virtual ~CorrespondenceGPSFix() - { - //delete cost_function_ptr_; - }; - -// double * getLocation() -// { -// return location_.data(); -// } - - virtual void addToProblem(ceres::Problem & _problem) - { - ceres_residual_block_id_ = _problem.AddResidualBlock(cost_function_ptr_,nullptr,state_position_.data()); - } - - virtual void display() const - { - std::cout << "---- GPS Fix Correspondence ----" << std::endl; - CorrespondenceBaseX::display(); - std::cout << "state_position_: " << state_position_.transpose() << std::endl; - std::cout << "gps_fix_: " << gps_fix_.transpose() << std::endl; - }; -}; - - -int main(int argc, char** argv) -{ - //Welcome message - std::cout << std::endl << " ========= WOLF-CERES test. Simple Odometry + GPS fix problem (with non-template classes) ===========" << std::endl << std::endl; - - //user input - if (argc!=3) - { - std::cout << "Please call me with: [./test_ceres_odom_iterative NI NW], where:" << std::endl; - std::cout << " - NI is the number of iterations" << std::endl; - std::cout << " - NW is the size of the window" << std::endl; - std::cout << "EXIT due to bad user input" << std::endl << std::endl; - return -1; - } - unsigned int n_execution = (unsigned int) atoi(argv[1]); //number of iterations of the whole execution - unsigned int n_window = (unsigned int) atoi(argv[2]); //size of the window. - - //init google log - google::InitGoogleLogging(argv[0]); - - //variables - unsigned int ii, jj, jj_previous; //iterators - Eigen::VectorXs odom_inc_true;//invented motion - Eigen::Vector3s pose_true; //current true pose - Eigen::VectorXs ground_truth; //accumulated true poses - Eigen::Vector3s pose_predicted; // current predicted pose - Eigen::VectorXs state; //running window winth solver result - Eigen::VectorXs state_prior; //state prior, just before solving the problem - Eigen::Vector2s odom_reading; //current odometry reading - Eigen::Vector3s gps_fix_reading; //current GPS fix reading - Eigen::VectorXs gps_log; //log of all gps readings - Eigen::VectorXs results_log; //log of optimized poses - CorrespondenceOdom2D *odom_corresp; //pointer to odometry correspondence - CorrespondenceGPSFix *gps_fix_corresp; //pointer to GPS fix correspondence - ceres::Problem problem; //ceres problem - ceres::Solver::Options options; //ceres solver options - ceres::Solver::Summary summary; //ceres solver summary - std::ofstream log_file; //output file - - //resize vectors according user input number of iterations - odom_inc_true.resize(n_execution*2); //2 odometry components per iteration - ground_truth.resize(n_execution*3);// 3 components per iteration - gps_log.resize(n_execution*3); //3 components per iteration - results_log.resize(n_execution*3); //3 components per iteration - state.resize(n_window*3); //3 components per window element - state_prior.resize(n_execution*3); //3 components per window element - - //init true odom and true pose - for (ii = 0; ii<n_execution; ii++) - { - if ( ii < (unsigned int)floor(n_execution/2) ) - odom_inc_true.middleRows(ii*2,2) << fabs(cos(ii/10.)) , fabs(sin(ii/2000.)); //invented motion increments. - else - odom_inc_true.middleRows(ii*2,2) << fabs(cos(ii/10.)) , -fabs(sin((ii-floor(n_execution/2))/2000.)); //invented motion increments. - } - pose_true << 0,0,0; - pose_predicted << 0,0,0; - ground_truth.middleRows(0,3) << pose_true; //init point pushed to ground truth - state.middleRows(0,3) << 0,0,0; //init state at origin - - //init random generators - std::default_random_engine generator; - std::normal_distribution<WolfScalar> distribution_odom(0.001,0.01); //odometry noise - std::normal_distribution<WolfScalar> distribution_gps(0.0,1); //GPS noise - - //test loop - for (ii = 1; ii<n_execution; ii++) //ii over iterations, jj over the window - { - //set jj index (over the window) - jj = ii%n_window; - jj_previous = (ii-1)%n_window; - - //inventing a simple motion - pose_true(0) = pose_true(0) + odom_inc_true(ii*2) * cos(pose_true(2)+odom_inc_true(ii*2+1)); - pose_true(1) = pose_true(1) + odom_inc_true(ii*2) * sin(pose_true(2)+odom_inc_true(ii*2+1)); - pose_true(2) = pose_true(2) + odom_inc_true(ii*2+1); - ground_truth.middleRows(ii*3,3) << pose_true; - //std::cout << "pose_true(" << ii << ") = " << pose_true.transpose() << std::endl; - - //inventing sensor readings for odometry and GPS - odom_reading << odom_inc_true(ii*2)+distribution_odom(generator), odom_inc_true(ii*2+1)+distribution_odom(generator); //true range and theta with noise - gps_fix_reading << pose_true(0) + distribution_gps(generator), pose_true(1) + distribution_gps(generator), 0. + distribution_gps(generator); - gps_log.middleRows(ii*3,3) << gps_fix_reading;//log the reading - - //setting initial guess from the last optimized pose, using noisy odometry - pose_predicted(0) = state(jj_previous*3) + odom_reading(0) * cos(state(jj_previous*3+2)+odom_reading(1)); - pose_predicted(1) = state(jj_previous*3+1) + odom_reading(0) * sin(state(jj_previous*3+2)+odom_reading(1)); - pose_predicted(2) = state(jj_previous*3+2) + odom_reading(1); - - //window management - state.middleRows(jj*3,3) << pose_predicted; - - //creating odom correspondence. Adding it to the problem - odom_corresp = new CorrespondenceOdom2D(state.data()+jj_previous*3, state.data()+jj*3, odom_reading); - odom_corresp->addToProblem(problem); - //odom_corresp->display(); - delete odom_corresp; - - //creating gps correspondence and adding it to the problem - gps_fix_corresp = new CorrespondenceGPSFix(state.data()+ii*3, gps_fix_reading); - gps_fix_corresp->addToProblem(problem); - //gps_fix_corresp->display(); - delete gps_fix_corresp; - - //set options and solve (sliding window) -// options.minimizer_progress_to_stdout = true; -// ceres::Solve(options, &problem, &summary); - } - - //display initial guess - //std::cout << "INITIAL GUESS IS: " << state.transpose() << std::endl; - - //set options and solve (batch mode) - //options.minimizer_progress_to_stdout = true; - state_prior = state; - options.minimizer_type = ceres::LINE_SEARCH;//ceres::TRUST_REGION; - options.max_line_search_step_contraction = 1e-3; - ceres::Solve(options, &problem, &summary); - - //display/log results, by setting cout flags properly - std::string filename( getenv("HOME") ); - filename += "/Desktop/log_data.txt"; - std::cout << std::endl << " Result to file " << filename << std::endl; - log_file.open(filename, std::ofstream::out); //open log file - for (unsigned int ii = 0; ii<n_execution; ii++) - log_file << state.middleRows(ii*3,3).transpose() << " " << ground_truth.middleRows(ii*3,3).transpose() << " " << (state.middleRows(ii*3,3)-ground_truth.middleRows(ii*3,3)).transpose() << " " << gps_log.middleRows(ii*3,3).transpose() << " " << state_prior.middleRows(ii*3,3).transpose() <<std::endl; - log_file.close(); //close log file - - //free memory (not necessary since ceres::problem holds their ownership) -// delete odom_corresp; -// delete gps_fix_corresp; - - //End message - std::cout << " =========================== END ===============================" << std::endl << std::endl; - - //exit - return 0; -} diff --git a/src/examples/test_ceres_wrapper_jet.cpp b/src/examples/test_ceres_wrapper_jet.cpp deleted file mode 100644 index e2dbd1e85efe6f44040c19b1cffd6074b0966dcd..0000000000000000000000000000000000000000 --- a/src/examples/test_ceres_wrapper_jet.cpp +++ /dev/null @@ -1,216 +0,0 @@ -//std includes -#include <cstdlib> -#include <iostream> -#include <fstream> -#include <memory> -#include <random> -#include <typeinfo> -#include <ctime> -#include <queue> - -// Eigen includes -#include <eigen3/Eigen/Dense> -#include <eigen3/Eigen/Geometry> - -//Ceres includes -#include "ceres/jet.h" -#include "ceres/ceres.h" -#include "glog/logging.h" - -//Wolf includes -#include "sensor_base.h" -#include "frame_base.h" -#include "capture_base.h" -#include "state_block.h" -#include "wolf.h" - -// ceres wrapper includes -#include "ceres_wrapper/complex_angle_parametrization.h" - -//Wolf includes -#include "wolf.h" -#include "node_terminus.h" -#include "node_linked.h" - -//namespaces -using namespace std; -using namespace Eigen; - -/** - * This class emulates a Wolf top tree node class, such as vehicle. - * This class will be removed when effective linking to Wolf, and using actual Vehicle class. - * It holds: - * - a map to a state vector - * - a map to an error vector - * - a method to compute the error from the state - * - **/ - -class WolfVehicle -{ - protected: - VectorXs state_prior_; //state storage where to compute prior - Map<VectorXs> state_map_; //state point to be evaluated by Wolf tree constraints - - //Just to generate fake measurements - std::default_random_engine generator_; //just to generate measurements - std::normal_distribution<double> distribution_; //just to generate measurements - MatrixXs measurements_; //just a set of invented measurements - - unsigned int measurements_size_, state_size_; - - public: - WolfVehicle() : - state_prior_(), - state_map_(nullptr,0), - distribution_(0.0,0.01), - measurements_size_(0), - state_size_(0) - { - // - } - - virtual ~WolfVehicle() - { - // - } - - WolfScalar *getPrior() - { - return state_prior_.data(); - } - - void resizeState(unsigned int _state_size) - { - state_size_ = _state_size; - state_prior_.resize(_state_size); - } - - void inventMeasurements(unsigned int _sz) - { - // for testing - measurements_size_ = _sz; - measurements_.resize(state_size_,measurements_size_); - - for(unsigned int ii=0; ii<measurements_size_*state_size_; ii++) - measurements_(ii) = 1 + distribution_(generator_); //just inventing a sort of noise measurements - - //std::cout << "invented measurements_ = " << std::endl << measurements_.transpose() << std::endl; - } - - void computePrior() - { - state_prior_.setZero(); - } - - template <typename T> - bool operator()(const T* const _x, T* _residuals) const - { - // Remap the vehicle state to the const evaluation point - Map<const Matrix<T,Dynamic,1>> state_map_const_(_x, state_size_); - - // Map residuals vector to matrix (with sizes of the measurements matrix) - Map<Matrix<T,Dynamic,Dynamic>> mapped_residuals(_residuals, state_size_, measurements_size_); - - // Compute error or residuals - mapped_residuals = measurements_.cast<T>() - state_map_const_.replicate(1,measurements_size_); - - // std::cout << typeid(T).name() << std::endl; - // for (unsigned int j = 0; j<state_size_; j++) - // { - // std::cout << "_x(" << j <<") = " << _x[j] << std::endl; - // for (unsigned int i = 0; i<measurements_size_; i++) - // std::cout << "mapped_residuals(" << j << "," << i <<") = " << mapped_residuals(j,i) << std::endl; - // } - - return true; - } - -// bool operator()(const WolfScalar* const _x, WolfScalar* _residuals) const -// { -// // Remap the vehicle state to the const evaluation point -// Map<const VectorXs> state_map_const_(_x, state_size_); -// -// // Map residuals vector to matrix (with sizes of the measurements matrix) -// Map<MatrixXs> mapped_residuals(_residuals, state_size_, measurements_size_); -// -// // Compute error or residuals -// mapped_residuals = measurements_ - state_map_const_.replicate(1,measurements_size_); -// -// return true; -// } - - void print() - { - //std::cout << "measurements_: " << std::endl << measurements_.transpose() << std::endl << std::endl; - std::cout << "state_prior_ : " << std::endl << state_prior_.transpose() << std::endl << std::endl; - //std::cout << "state_const_ : " << std::endl << state_map_const_.transpose() << std::endl << std::endl; - } -}; - -//This main is a single iteration of the WOLF. -//Once at ROS, Calls to WolfVehicle, CeresWolfFunctor and Ceres objects will be executed in a similar order in a function of the ROS wrapper -int main(int argc, char** argv) -{ - std::cout << " ========= Static Numeric case ===========" << std::endl << std::endl; - -// NodeLinked<NodeTerminus,NodeTerminus> node(TOP,"TRAJECTORY"); - - //dimension - const unsigned int STATE_DIM = 5; //just to test, all will be DIM-dimensional - const unsigned int MEASUREMENT_DIM = 1; //just to test, all will be DIM-dimensional - - // init - google::InitGoogleLogging(argv[0]); - - using ceres::AutoDiffCostFunction; - using ceres::CostFunction; - - //wolf vehicle & ceres functor - WolfVehicle *functorPtr = new WolfVehicle(); - - // Ceres problem initialization - ceres::Solver::Options options; - options.minimizer_progress_to_stdout = false; - options.minimizer_type = ceres::TRUST_REGION; - options.line_search_direction_type = ceres::LBFGS; - options.max_num_iterations = 10; - ceres::Solver::Summary summary; - ceres::Problem problem; - - // fixed dim problem - functorPtr->resizeState(STATE_DIM); - functorPtr->computePrior(); - - // set measures. This will be replaced by the WOLF-ROS front-end, getting sensor readings from sensors and performing measurements to build the whole wolf tree - functorPtr->inventMeasurements(MEASUREMENT_DIM); - - // cost function - CostFunction* cost_function = new AutoDiffCostFunction<WolfVehicle,1,1>(functorPtr); - for (unsigned int st=0; st < STATE_DIM; st++) - { - problem.AddResidualBlock(cost_function, NULL, functorPtr->getPrior()+st); - } - //CostFunction* cost_function = new AutoDiffCostFunction<WolfVehicle,STATE_DIM*MEASUREMENT_DIM,STATE_DIM>(functorPtr); - //problem.AddResidualBlock(cost_function, NULL, functorPtr->getPrior()); - - // run Ceres Solver - ceres::Solve(options, &problem, &summary); - - //display results - std::cout << summary.BriefReport() << "\n"; - functorPtr->print(); - - //clean - std::cout << "Cleaning ... " << std::endl << std::endl; - //problem.RemoveResidualBlock(rbId); - delete cost_function; - delete functorPtr; - - //end Wolf iteration - std::cout << " ========= END ===========" << std::endl << std::endl; - - //exit - return 0; -} - diff --git a/src/examples/test_ceres_wrapper_jet_2_sensors.cpp b/src/examples/test_ceres_wrapper_jet_2_sensors.cpp deleted file mode 100644 index 5fcfc457b6fc7aa9e70934dff25fe5f04fad20da..0000000000000000000000000000000000000000 --- a/src/examples/test_ceres_wrapper_jet_2_sensors.cpp +++ /dev/null @@ -1,419 +0,0 @@ -//std includes -#include <iostream> -#include <memory> -#include <random> -#include <typeinfo> - -// Eigen includes -#include <eigen3/Eigen/Dense> -#include <eigen3/Eigen/Geometry> - -//Ceres includes -#include "ceres/jet.h" -#include "ceres/ceres.h" -#include "ceres/loss_function.h" -#include "glog/logging.h" - -//Wolf includes -#include "wolf.h" - -/** - * This class emulates a Wolf top tree node class, such as vehicle. - * This class will be removed when effective linking to Wolf, and using actual Vehicle class. - * It holds: - * - a map to a state vector - * - a map to an error vector - * - a method to compute the error from the state - * - **/ -using namespace Eigen; - -class CorrespondenceBase -{ - protected: - VectorXs measurement_; - - public: - - CorrespondenceBase(const unsigned int & _measurement_size) : - measurement_(_measurement_size) - { - } - - virtual ~CorrespondenceBase() - { - } - - virtual void inventMeasurement(const VectorXs& _measurement, std::default_random_engine& _generator, std::normal_distribution<WolfScalar>& _distribution) - { - measurement_ = _measurement; - for(unsigned int ii=0; ii<measurement_.size(); ii++) - measurement_(ii) += _distribution(_generator); //just inventing a sort of noise measurements - //std::cout << "measurement_" << measurement_ << std::endl; - } -}; - -class CorrespondenceCeresBase -{ - protected: - ceres::CostFunction* cost_function_; - - public: - - CorrespondenceCeresBase() - { - } - - virtual ~CorrespondenceCeresBase() - { - } - - virtual void addBlock(ceres::Problem & ceres_problem) = 0; -}; - -template <unsigned int MEASUREMENT_SIZE = 1, unsigned int BLOCK_SIZE = 1> -class Correspondence1Sparse: public CorrespondenceBase -{ - protected: - Map<Matrix<WolfScalar, BLOCK_SIZE, 1>> state_block_map_; - - public: - - Correspondence1Sparse(WolfScalar* _statePtr) : - CorrespondenceBase(MEASUREMENT_SIZE), - state_block_map_(_statePtr,BLOCK_SIZE) - { - } - - virtual ~Correspondence1Sparse() - { - } - - virtual WolfScalar* getBlockPtr() - { - return state_block_map_.data(); - } - - template <typename T> - //void compute_residuals(const Matrix<T,Dynamic,1>& _x, Matrix<T,Dynamic,1> residuals) - void compute_residuals(Map<const Matrix<T,Dynamic,1>>& _st, Map<Matrix<T,Dynamic,1>> residuals) const - { - residuals = measurement_.cast<T>() - _st; - // std::cout << typeid(T).name() << std::endl; - // for (unsigned int j = 0; j<BLOCK_SIZE; j++) - // { - // std::cout << "_x(" << j <<") = " << _x[j] << std::endl; - // std::cout << "mapped_residuals(" << j <<") = " << mapped_residuals(j) << std::endl; - // } - } -}; - -template <unsigned int MEASUREMENT_SIZE = 1, unsigned int BLOCK_SIZE = 1> -class Correspondence1SparseCeres: public CorrespondenceCeresBase, public Correspondence1Sparse<MEASUREMENT_SIZE, BLOCK_SIZE> -{ - public: - - Correspondence1SparseCeres(WolfScalar* _statePtr) : - CorrespondenceCeresBase(), - Correspondence1Sparse<MEASUREMENT_SIZE, BLOCK_SIZE>(_statePtr) - { - cost_function_ = new ceres::AutoDiffCostFunction<Correspondence1SparseCeres,MEASUREMENT_SIZE,BLOCK_SIZE>(this); - } - - virtual ~Correspondence1SparseCeres() - { - } - - template <typename T> - bool operator()(const T* const _x, T* _residuals) const - { - //std::cout << "adress of x: " << _x << std::endl; - - // Remap the vehicle state to the const evaluation point - Map<const Matrix<T,Dynamic,1>> state_map_const(_x, BLOCK_SIZE); - - // Map residuals vector to matrix (with sizes of the measurements matrix) - Map<Matrix<T,Dynamic,1>> mapped_residuals(_residuals, MEASUREMENT_SIZE); - - // Compute error or residuals - this->template compute_residuals<T>(state_map_const, mapped_residuals); - - return true; - } - - virtual void addBlock(ceres::Problem & ceres_problem) - { - //std::cout << " adding correspondence_1_sparse_ceres..."; - ceres_problem.AddResidualBlock(cost_function_, NULL, this->state_block_map_.data()); - //std::cout << " added!" << std::endl; - } -}; - - - -template <unsigned int MEASUREMENT_SIZE = 1, unsigned int BLOCK_1_SIZE = 1, unsigned int BLOCK_2_SIZE = 1> -class Correspondence2Sparse: public CorrespondenceBase -{ - protected: - Map<Matrix<WolfScalar, BLOCK_1_SIZE, 1>> state_block_1_map_; - Map<Matrix<WolfScalar, BLOCK_2_SIZE, 1>> state_block_2_map_; - - public: - - Correspondence2Sparse(WolfScalar* _block1Ptr, WolfScalar* _block2Ptr) : - CorrespondenceBase(MEASUREMENT_SIZE), - state_block_1_map_(_block1Ptr,BLOCK_1_SIZE), - state_block_2_map_(_block2Ptr,BLOCK_2_SIZE) - { - } - - virtual ~Correspondence2Sparse() - { - } - - template <typename T> - void compute_residuals(Map<const Matrix<T,Dynamic,1>>& _st1, Map<const Matrix<T,Dynamic,1>>& _st2, Map<Matrix<T,Dynamic,1>> residuals) const - { - Matrix<T,Dynamic,1> expected_measurement = ((_st1 - _st2).transpose() * (_st1 - _st2)).cwiseSqrt(); - VectorXd meas = this->measurement_; - residuals = (meas).cast<T>() - expected_measurement; - } - - WolfScalar *getBlock1Ptr() - { - return state_block_1_map_.data(); - } - - WolfScalar *getBlock2Ptr() - { - return state_block_2_map_.data(); - } -}; - -template <unsigned int MEASUREMENT_SIZE = 1, unsigned int BLOCK_1_SIZE = 1, unsigned int BLOCK_2_SIZE = 1> -class Correspondence2SparseCeres: public CorrespondenceCeresBase, public Correspondence2Sparse<MEASUREMENT_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE> -{ - public: - - Correspondence2SparseCeres(WolfScalar* _block1Ptr, WolfScalar* _block2Ptr) : - CorrespondenceCeresBase(), - Correspondence2Sparse<MEASUREMENT_SIZE, BLOCK_1_SIZE, BLOCK_2_SIZE>(_block1Ptr, _block2Ptr) - { - cost_function_ = new ceres::AutoDiffCostFunction<Correspondence2SparseCeres,MEASUREMENT_SIZE,BLOCK_1_SIZE,BLOCK_2_SIZE>(this); - } - - virtual ~Correspondence2SparseCeres() - { - } - - template <typename T> - bool operator()(const T* const _x1, const T* const _x2, T* _residuals) const - { - // print inputs - // std::cout << "_x1 = "; - // for (int i=0; i < BLOCK_1_SIZE; i++) - // std::cout << _x1[i] << " "; - // std::cout << std::endl; - // std::cout << "_x2 = "; - // for (int i=0; i < BLOCK_2_SIZE; i++) - // std::cout << _x2[i] << " "; - // std::cout << std::endl; - // std::cout << "measurement = "; - // for (int i=0; i < MEASUREMENT_SIZE; i++) - // std::cout << this->measurement_(i) << " "; - // std::cout << std::endl; - - // Remap the vehicle state to the const evaluation point - Map<const Matrix<T,Dynamic,1>> x1_map_const(_x1, BLOCK_1_SIZE); - Map<const Matrix<T,Dynamic,1>> x2_map_const(_x2, BLOCK_2_SIZE); - - // Map residuals vector to matrix (with sizes of the measurements matrix) - Map<Matrix<T,Dynamic,1>> mapped_residuals(_residuals, MEASUREMENT_SIZE); - - // Compute error or residuals - this->template compute_residuals(x1_map_const, x2_map_const, mapped_residuals); - - // print outputs - // std::cout << "expected = "; - // for (int i=0; i < MEASUREMENT_SIZE; i++) - // std::cout << expected_measurement(i) << " "; - // std::cout << std::endl; - // std::cout << "_residuals = "; - // for (int i=0; i < MEASUREMENT_SIZE; i++) - // std::cout << _residuals[i] << " "; - // std::cout << std::endl << std::endl; - - return true; - } - - virtual void addBlock(ceres::Problem & ceres_problem) - { - //std::cout << " adding correspondence_2_sparse_ceres..."; - ceres_problem.AddResidualBlock(cost_function_, NULL, this->state_block_1_map_.data(), this->state_block_2_map_.data()); - //std::cout << " added!" << std::endl; - } -}; - -class WolfProblem -{ - protected: - VectorXs state_; //state storage - std::vector<CorrespondenceCeresBase*> correspondences_; - - unsigned int state_size_; - - public: - WolfProblem() : - state_(), - correspondences_(0), - state_size_(0) - { - } - - virtual ~WolfProblem() - { - } - - WolfScalar *getPrior() - { - return state_.data(); - } - - CorrespondenceCeresBase* getCorrespondence(const unsigned int _idx) - { - //std::cout << correspondences_.size() << " correspondences" << std::endl; - return correspondences_[_idx]; - } - - unsigned int getCorrespondencesSize() - { - return correspondences_.size(); - } - - void resizeState(unsigned int _state_size) - { - state_size_ = _state_size; - state_.resize(_state_size); - } - - void addCorrespondence(CorrespondenceCeresBase* _absCorrPtr) - { - correspondences_.push_back(_absCorrPtr); - //std::cout << correspondences_.size() << " correspondence added!" << std::endl; - } - - void computePrior() - { - state_.setRandom(); - } - - void print() - { - std::cout << "state_ : " << std::endl << state_.transpose() << std::endl << std::endl; - } -}; - -//This main is a single iteration of the WOLF. -//Once at ROS, Calls to WolfVehicle, CeresWolfFunctor and Ceres objects will be executed in a similar order in a function of the ROS wrapper -int main(int argc, char** argv) -{ - std::cout << " ========= Static Numeric case ===========" << std::endl << std::endl; - - //dimension - const unsigned int DIM = 3; - const unsigned int N_STATES = 10; - const unsigned int STATE_DIM = DIM * N_STATES; - const unsigned int MEAS_A_DIM = 3; - const unsigned int MEAS_B_DIM = 1; - const unsigned int N_MEAS_A = 100; - const unsigned int N_MEAS_B = 50; - //const double w_A = 1; - //const double w_B = 10; - - // init - google::InitGoogleLogging(argv[0]); - std::default_random_engine generator; - std::normal_distribution<WolfScalar> distribution_A(0.0,0.01); - std::normal_distribution<WolfScalar> distribution_B(0.0,0.1); - VectorXs actualState(STATE_DIM); - for (unsigned int i=0;i<STATE_DIM; i++) - actualState(i) = i; - - using ceres::AutoDiffCostFunction; - using ceres::CostFunction; - - // Ceres problem initialization - ceres::Solver::Options options; - options.minimizer_progress_to_stdout = false; - options.minimizer_type = ceres::TRUST_REGION; - options.line_search_direction_type = ceres::LBFGS; - options.max_num_iterations = 100; - ceres::Solver::Summary summary; - ceres::Problem ceres_problem; - - //wolf problem - WolfProblem *wolf_problem = new WolfProblem(); - wolf_problem->resizeState(STATE_DIM); - wolf_problem->computePrior(); - std::cout << "Real state: " << actualState.transpose() << std::endl; - wolf_problem->print(); - - // Correspondences - // SENSOR A: Absolute measurements of the whole state - for(unsigned int mA=0; mA < N_MEAS_A; mA++) - { - for (unsigned int st=0; st < N_STATES; st++) - { - Correspondence1SparseCeres<MEAS_A_DIM, DIM>* corrAPtr = new Correspondence1SparseCeres<MEAS_A_DIM, DIM>(wolf_problem->getPrior()+st*DIM); - VectorXs actualMeasurement = actualState.segment(st*DIM,DIM); - corrAPtr->inventMeasurement(actualMeasurement,generator,distribution_A); - wolf_problem->addCorrespondence(corrAPtr); - } - } - // SENSOR B: Relative distances between points - for(unsigned int mB=0; mB < N_MEAS_B; mB++) - { - for (unsigned int st_from=0; st_from < N_STATES-1; st_from++) - { - for (unsigned int st_to=st_from+1; st_to < N_STATES; st_to++) - { - Correspondence2SparseCeres<MEAS_B_DIM, DIM, DIM>* corrBPtr = new Correspondence2SparseCeres<MEAS_B_DIM, DIM, DIM>(wolf_problem->getPrior()+st_from*DIM,wolf_problem->getPrior()+st_to*DIM); - VectorXs actualMeasurement = ((actualState.segment(st_from*DIM,DIM) - actualState.segment(st_to*DIM,DIM)).transpose() * (actualState.segment(st_from*DIM,DIM) - actualState.segment(st_to*DIM,DIM))).cwiseSqrt(); - corrBPtr->inventMeasurement(actualMeasurement,generator,distribution_B); - wolf_problem->addCorrespondence(corrBPtr); - } - } -// correspondence_2_sparse_ceres<MEAS_B_DIM, DIM, DIM>* corrBPtr = new correspondence_2_sparse_ceres<MEAS_B_DIM, DIM, DIM>(wolf_problem->getPrior(),wolf_problem->getPrior()+DIM); -// VectorXs actualMeasurement = ((actualState.head(DIM) - actualState.tail(DIM)).transpose() * (actualState.head(DIM) - actualState.tail(DIM))).cwiseSqrt(); -// corrBPtr->inventMeasurement(actualMeasurement,generator,distribution_B); -// wolf_problem->addCorrespondence(corrBPtr); - } - - // cost function - std::cout << "Number of blocks: " << std::endl << wolf_problem->getCorrespondencesSize() << std::endl; - for (unsigned int block=0; block < wolf_problem->getCorrespondencesSize(); block++) - { - //std::cout << "block " << block << "..."; - wolf_problem->getCorrespondence(block)->addBlock(ceres_problem); - //std::cout << " added!" << std::endl; - } - - // run Ceres Solver - ceres::Solve(options, &ceres_problem, &summary); - - //display results - std::cout << summary.FullReport() << "\n"; - wolf_problem->print(); - - //clean - std::cout << "Cleaning ... " << std::endl << std::endl; - //ceres_problem.RemoveResidualBlock(rbId); - delete wolf_problem; - - //end Wolf iteration - std::cout << " ========= END ===========" << std::endl << std::endl; - - //exit - return 0; -} - diff --git a/src/examples/test_ceres_wrapper_jet_ind_block.cpp b/src/examples/test_ceres_wrapper_jet_ind_block.cpp deleted file mode 100644 index ce98b4ed5748fde1294b694ce1a0dda25516fa9e..0000000000000000000000000000000000000000 --- a/src/examples/test_ceres_wrapper_jet_ind_block.cpp +++ /dev/null @@ -1,214 +0,0 @@ -//std includes -#include <iostream> -#include <memory> -#include <random> - -// Eigen includes -#include <eigen3/Eigen/Dense> -#include <eigen3/Eigen/Geometry> - -//Ceres includes -#include "ceres/ceres.h" -#include "glog/logging.h" - -//Wolf includes -#include "wolf.h" - -/** - * This class emulates a Wolf top tree node class, such as vehicle. - * This class will be removed when effective linking to Wolf, and using actual Vehicle class. - * It holds: - * - a map to a state vector - * - a map to an error vector - * - a method to compute the error from the state - * - **/ -using namespace Eigen; - -class AbsoluteCorrespondence -{ - protected: - Map<VectorXs> state_block_mapped_; - - //Just to generate fake measurements - std::default_random_engine generator_; //just to generate measurements - std::normal_distribution<WolfScalar> distribution_; //just to generate measurements - VectorXs measurement_; // invented measurement - ceres::CostFunction* cost_function; - - public: - - AbsoluteCorrespondence(WolfScalar* _statePtr, const unsigned int & _block_size) : - state_block_mapped_(_statePtr,_block_size), - distribution_(0.0,0.01), - measurement_(_block_size), - cost_function(new ceres::AutoDiffCostFunction<AbsoluteCorrespondence,1,1>(this)) - { - } - - virtual ~AbsoluteCorrespondence() - { - } - - WolfScalar *getPrior() - { - return state_block_mapped_.data(); - } - - void inventMeasurement(std::default_random_engine& _generator) - { - for(unsigned int ii=0; ii<state_block_mapped_.size(); ii++) - measurement_(ii) = 1 + distribution_(_generator); //just inventing a sort of noise measurements - } - - template <typename T> - bool operator()(const T* const _x, T* _residuals) const - { - // Remap the vehicle state to the const evaluation point - Map<const Matrix<T,Dynamic,1>> state_map_const_(_x, state_block_mapped_.size()); - - // Map residuals vector to matrix (with sizes of the measurements matrix) - Map<Matrix<T,Dynamic,1>> mapped_residuals(_residuals, state_block_mapped_.size()); - - // Compute error or residuals - mapped_residuals = measurement_.cast<T>() - state_map_const_; - - // std::cout << typeid(T).name() << std::endl; - // for (unsigned int j = 0; j<block_size_; j++) - // { - // std::cout << "_x(" << j <<") = " << _x[j] << std::endl; - // std::cout << "mapped_residuals(" << j <<") = " << mapped_residuals(j) << std::endl; - // } - - return true; - } - - void addBlock(ceres::Problem & ceres_problem) - { - ceres_problem.AddResidualBlock(cost_function, NULL, getPrior()); - } -}; - -class WolfProblem -{ - protected: - VectorXs state_; //state storage - std::vector<AbsoluteCorrespondence*> correspondences_; - - unsigned int state_size_; - - public: - WolfProblem() : - state_(), - correspondences_(0), - state_size_(0) - { - } - - virtual ~WolfProblem() - { - } - - WolfScalar *getPrior() - { - return state_.data(); - } - - AbsoluteCorrespondence* getCorrespondence(const unsigned int _idx) - { - return correspondences_[_idx]; - } - - unsigned int getCorrespondencesSize() - { - return correspondences_.size(); - } - - void resizeState(unsigned int _state_size) - { - state_size_ = _state_size; - state_.resize(_state_size); - } - - void addCorrespondence(AbsoluteCorrespondence* _absCorrPtr) - { - correspondences_.push_back(_absCorrPtr); - } - - void inventMeasurements(std::default_random_engine& _generator) - { - for (std::vector<AbsoluteCorrespondence*>::iterator it = correspondences_.begin(); it != correspondences_.end(); it++) - (*it)->inventMeasurement(_generator); - } - - void computePrior() - { - state_.setZero(); - } - - void print() - { - std::cout << "state_ : " << std::endl << state_.transpose() << std::endl << std::endl; - } -}; - -//This main is a single iteration of the WOLF. -//Once at ROS, Calls to WolfVehicle, CeresWolfFunctor and Ceres objects will be executed in a similar order in a function of the ROS wrapper -int main(int argc, char** argv) -{ - std::cout << " ========= Static Numeric case ===========" << std::endl << std::endl; - - //dimension - const unsigned int STATE_DIM = 50; //just to test, all will be DIM-dimensional - const unsigned int N_MEASUREMENTS = 1000; - // init - google::InitGoogleLogging(argv[0]); - std::default_random_engine generator; - - using ceres::AutoDiffCostFunction; - using ceres::CostFunction; - - // Ceres problem initialization - ceres::Solver::Options options; - options.minimizer_progress_to_stdout = false; - options.minimizer_type = ceres::TRUST_REGION; - options.line_search_direction_type = ceres::LBFGS; - options.max_num_iterations = 10; - ceres::Solver::Summary summary; - ceres::Problem ceres_problem; - - //wolf problem - WolfProblem *wolf_problem = new WolfProblem(); - wolf_problem->resizeState(STATE_DIM); - wolf_problem->computePrior(); - for(unsigned int st=0; st < N_MEASUREMENTS; st++) - for (unsigned int st=0; st < STATE_DIM; st++) - wolf_problem->addCorrespondence(new AbsoluteCorrespondence(wolf_problem->getPrior()+st,1)); - - // set measures. This will be replaced by the WOLF-ROS front-end, getting sensor readings from sensors and performing measurements to build the whole wolf tree - wolf_problem->inventMeasurements(generator); - - // cost function - std::cout << "Number of blocks: " << std::endl << wolf_problem->getCorrespondencesSize() << std::endl; - for (unsigned int block=0; block < wolf_problem->getCorrespondencesSize(); block++) - wolf_problem->getCorrespondence(block)->addBlock(ceres_problem); - - // run Ceres Solver - ceres::Solve(options, &ceres_problem, &summary); - - //display results - std::cout << summary.FullReport() << "\n"; - wolf_problem->print(); - - //clean - std::cout << "Cleaning ... " << std::endl << std::endl; - //ceres_problem.RemoveResidualBlock(rbId); - delete wolf_problem; - - //end Wolf iteration - std::cout << " ========= END ===========" << std::endl << std::endl; - - //exit - return 0; -} - diff --git a/src/examples/test_ceres_wrapper_numeric.cpp b/src/examples/test_ceres_wrapper_numeric.cpp deleted file mode 100644 index 60528eafb5938967a443454f29132c5658f46a94..0000000000000000000000000000000000000000 --- a/src/examples/test_ceres_wrapper_numeric.cpp +++ /dev/null @@ -1,220 +0,0 @@ -//std includes -#include <iostream> -#include <memory> -#include <random> - -// Eigen includes -#include <eigen3/Eigen/Dense> -#include <eigen3/Eigen/Geometry> - -//Ceres includes -#include "ceres/ceres.h" -#include "glog/logging.h" - -//Wolf includes -#include "wolf.h" - -/** - * This class emulates a Wolf top tree node class, such as vehicle. - * This class will be removed when effective linking to Wolf, and using actual Vehicle class. - * It holds: - * - a map to a state vector - * - a map to an error vector - * - a method to compute the error from the state - * - **/ -class WolfVehicle -{ - protected: - Eigen::VectorXs state_; //state storage where to compute prior, and where result is placed. Is the allocation for the state vector - Eigen::Map<const Eigen::VectorXs> state_map_; //state point to be evaluated by Wolf tree constraints - Eigen::Map<Eigen::VectorXs> error_map_; //error computed by the wolf tree - unsigned int state_size_; - unsigned int error_size_; - - //Just to generate fake measurements - std::default_random_engine generator_; //just to generate measurements - std::normal_distribution<double> distribution_; //just to generate measurements - Eigen::VectorXs measurements_; //just a set of invented measurements - - public: - WolfVehicle() : - state_(), - state_map_(nullptr,0), - error_map_(nullptr,0), - state_size_(0), - error_size_(0), - distribution_(0.0,0.1) - { - // - } - - virtual ~WolfVehicle() - { - // - } - - void setSizes(unsigned int _state_size, unsigned int _error_size) - { - state_size_ = _state_size; - error_size_ = _error_size; - state_.resize(_state_size); - } - - void remapState(const WolfScalar *_ptr) - { - new (&state_map_) Eigen::Map<const Eigen::VectorXs>(_ptr, state_size_); - } - - void remapError(WolfScalar *_ptr) - { - new (&error_map_) Eigen::Map<Eigen::VectorXs>(_ptr, error_size_); - } - - void inventMeasurements(unsigned int _sz) - { - measurements_.resize(_sz); - for(unsigned int ii=0; ii<_sz; ii++) - { - measurements_(ii) = 1 + distribution_(generator_); //just inventing a sort of noise measurements - } - } - - WolfScalar *getState() - { - return state_.data(); - } - - void getState(Eigen::VectorXs & _v) - { - _v.resize(state_.size()); - _v = state_; - } - - void computePrior() - { - state_.setOnes();//just a fake prior - } - - void computeError() - { - for(unsigned int ii=0; ii<error_map_.size(); ii++) - { - error_map_(ii) = measurements_(ii) - state_map_(ii); //just a trivial error function - } - } - - void print() - { - std::cout << "measurements_: " << measurements_.transpose() << std::endl; - std::cout << "state_: " << state_.transpose() << std::endl; - //std::cout << "state_map_: " << state_map_.transpose() << std::endl; - } -}; - -/** - * This class wrapps the Wolf Vehicle and implements the operator(), to be used by CERES optimizer - * - the operator() overloaded -**/ -class CeresWolfFunctor -{ - protected: - std::shared_ptr<WolfVehicle> vehicle_ptr_; //pointer to Wolf Vehicle object - - public: - CeresWolfFunctor(std::shared_ptr<WolfVehicle> & _wv) : - vehicle_ptr_(_wv) - { - //std::cout << "CeresWolfFunctor(): " << vehicle_ptr_.use_count() << " " << state_ptr_.use_count() << " " << std::endl; - } - - virtual ~CeresWolfFunctor() - { - // - } - bool operator()(const WolfScalar * const _x, double* _residuals) const - { - // 1. Remap the vehicle state to the provided x - vehicle_ptr_->remapState(_x); - - // 2. Remap the error to the provided address - vehicle_ptr_->remapError(_residuals); - - // 3. Compute error - vehicle_ptr_->computeError(); - - // 4. return - return true; - } - -}; - -//This main is a single iteration of the WOLF. -//Once at ROS, Calls to WolfVehicle, CeresWolfFunctor and Ceres objects will be executed in a similar order in a function of the ROS wrapper -int main(int argc, char** argv) -{ - std::cout << " ========= Static Numeric case ===========" << std::endl << std::endl; - - //dimension - const unsigned int DIM = 19; //just to test, all will be DIM-dimensional - - // init - google::InitGoogleLogging(argv[0]); - - //wolf vehicle - std::shared_ptr<WolfVehicle> vehiclePtr(std::make_shared<WolfVehicle>()); - - //ceres functor - CeresWolfFunctor *functorPtr = new CeresWolfFunctor(vehiclePtr); - - // Allocate the cost function !!!! Difference is to create in the same line both objects -> in the last (): (new ... ) SEE test_ceres_basic.cpp - ceres::NumericDiffCostFunction<CeresWolfFunctor,ceres::CENTRAL,DIM,DIM>* - cost_function_static = new ceres::NumericDiffCostFunction<CeresWolfFunctor,ceres::CENTRAL,DIM,DIM>(functorPtr); - - //********************** start Wolf iteration ************************* - - // set measures. This will be replaced by the WOLF-ROS front-end, getting sensor reading from sensors (callbacks) and performing measurements to build the whole wolf tree - vehiclePtr->inventMeasurements(DIM); - - // Resizing & remapping. Dimensions may come from a call to WolfVehicle::getSizes() - vehiclePtr->setSizes(DIM,DIM); - - // Compute and gets the Prior (initial guess). This would be the prior given by Wolf - vehiclePtr->computePrior(); - - // build Ceres problem - ceres::Problem *problem = new ceres::Problem(); - problem->AddResidualBlock(cost_function_static, nullptr, vehiclePtr->getState()); - - // run Ceres Solver - ceres::Solver::Options options; - options.minimizer_progress_to_stdout = true; - options.minimizer_type = ceres::TRUST_REGION; - options.line_search_direction_type = ceres::LBFGS; - options.max_num_iterations = 10; - ceres::Solver::Summary summary; - ceres::Solve(options, problem, &summary); - - //display results - std::cout << std::endl<< "Ceres Report:" << std::endl; - std::cout << summary.BriefReport() << "\n"; - std::cout << std::endl << "Wolf vectors:" << std::endl; - vehiclePtr->print(); - - //********************** end Wolf iteration ************************* - - //clean - //std::cout << "Cleaning ... " << std::endl << std::endl; - //something to do ?? - delete problem; - //delete vehiclePtr; - //delete cost_function_static; - //delete cost_function_static; - - //end Wolf iteration - std::cout << " ========= END ===========" << std::endl << std::endl; - - //exit - return 0; -} - diff --git a/src/examples/test_wolf_tree.cpp b/src/examples/test_wolf_tree.cpp index 7dfb31390a3e8edccdb7f0d78bf6f2fcd6840a50..3ff333680dde3574c55b9d9dd7c6c5a3b4ea1ecf 100644 --- a/src/examples/test_wolf_tree.cpp +++ b/src/examples/test_wolf_tree.cpp @@ -1,4 +1,4 @@ -// Testing a full wolf tree avoiding template classes for NodeLinked derived classes +// Testing create and delete full wolf tree with odometry captures //std includes #include <iostream> @@ -7,59 +7,40 @@ #include <cmath> #include <queue> -//ceres -#include "ceres/ceres.h" - //Wolf includes -#include "wolf.h" -#include "time_stamp.h" -#include "capture_base.h" -#include "sensor_base.h" +#include "wolf_manager.h" int main(int argc, char** argv) -{ - //wolf manager variables - std::queue<FrameBase> trajectory_; //this will be owned by the wolf manager - std::list<CaptureBaseShPtr> pending_captures_; //this will be owned by the wolf manager - std::list<CaptureBaseShPtr>::iterator pending_it_; //this will be owned by the wolf manager - Eigen::VectorXs sp(6); - sp << 0.1,0.1,0.1,0,0,0; - SensorBase sensor1(ABSOLUTE_POSE,sp); //just one sensor. This will be owned by the wolf manager - sp << 0.2,0.2,0.2,0,0,0; - SensorBase sensor2(ABSOLUTE_POSE,sp); //just another sensor. This will be owned by the wolf manager - - //ROS callbacks varaibles (will be get from message) - TimeStamp ros_ts; //this plays the role of ros::Time, or msg->header.stamp - Eigen::VectorXs sensor_reading(4); //this plays the role of the ROS message content (sensor reading). Reading of dim=4 (example) - +{ //Welcome message std::cout << std::endl << " ========= WOLF TREE test ===========" << std::endl << std::endl; + SensorOdom2D* odom_sensor_ptr_ = new SensorOdom2D(new StateBlock(Eigen::Vector3s::Zero()), + new StateBlock(Eigen::Vector1s::Zero()), 0.1, 0.1); + + WolfManager* wolf_manager_ = new WolfManager(PO_2D, //frame structure + odom_sensor_ptr_, //odom sensor + Eigen::Vector3s::Zero(), //prior + Eigen::Matrix3s::Identity()*0.01, //prior cov + 5, //window size + 1); //time for new keyframe + wolf_manager_->addSensor(odom_sensor_ptr_); + //main loop - for (unsigned int ii = 0; ii<10; ii++) + for (unsigned int ii = 0; ii<1000; ii++) { - //1. a new sensor data arrives (this part will be placed on ROS callbacks) - ros_ts.setToNow(); - sensor_reading << 1,2,3,4; - std::shared_ptr<CaptureBase> capture( new CaptureBase(ros_ts.get(), &sensor1, sensor_reading) ); - pending_captures_.push_back(capture); - - //2. Process pending_captures_, deciding for each new capture wheter a Frame has to be created or they have to be linked to the last one - for (pending_it_ = pending_captures_.begin(); pending_it_ != pending_captures_.end(); ++pending_it_) - { - capture->processCapture(); //This should create features - - } - - //3. Stablish correspondences - - //4. Call ceres solver - - //5. publish results - + // Add sintetic odometry + wolf_manager_->addCapture(new CaptureOdom2D(TimeStamp(ii*0.1), + TimeStamp(ii*0.1+0.01), + odom_sensor_ptr_, + Eigen::Vector3s(0.1, 0. ,0.05))); + // update wolf tree + wolf_manager_->update(); } - + + delete wolf_manager_; //not necessary to delete anything more, wolf will do it! + //End message std::cout << " =========================== END ===============================" << std::endl << std::endl;