From cbc69a7296c456ef5dd15b9b5f7c8166de17c911 Mon Sep 17 00:00:00 2001
From: joanvallve <jvallve@iri.upc.edu>
Date: Thu, 17 Dec 2015 11:07:55 +0100
Subject: [PATCH] AutoDiffCostFunctionWrapper tested with test_autodiff
---
src/examples/test_autodiff.cpp | 426 +++++++++++++++++++++++++++++++++
1 file changed, 426 insertions(+)
create mode 100644 src/examples/test_autodiff.cpp
diff --git a/src/examples/test_autodiff.cpp b/src/examples/test_autodiff.cpp
new file mode 100644
index 000000000..69fbaac1a
--- /dev/null
+++ b/src/examples/test_autodiff.cpp
@@ -0,0 +1,426 @@
+//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 "glog/logging.h"
+
+//Wolf includes
+#include "wolf_manager.h"
+#include "sensor_laser_2D.h"
+#include "processor_laser_2D.h"
+#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 <= 170) { displacement_ = 0.2; rotation_ = 1.8 * M_PI / 180; }
+ else if (ii <= 220) { displacement_ = 0; rotation_ =-1.8 * M_PI / 180; }
+ else if (ii <= 310) { displacement_ = 0.1; rotation_ = 0; }
+ else if (ii <= 487) { displacement_ = 0.1; rotation_ =-1.0 * M_PI / 180; }
+ else if (ii <= 600) { displacement_ = 0.2; rotation_ = 0; }
+ else if (ii <= 700) { displacement_ = 0.1; rotation_ =-1.0 * M_PI / 180; }
+ else if (ii <= 780) { displacement_ = 0; rotation_ =-1.0 * M_PI / 180; }
+ else { displacement_ = 0.3; rotation_ = 0; }
+
+ pose.moveForward(displacement_);
+ pose.rt.setEuler(pose.rt.head() + rotation_, pose.rt.pitch(), pose.rt.roll());
+}
+
+int main(int argc, char** argv)
+{
+ std::cout << "\n ========= 2D Robot with odometry and 2 LIDARs ===========\n";
+
+ // USER INPUT ============================================================================================
+ if (argc != 3 || atoi(argv[1]) < 1 || atoi(argv[2]) < 1 )
+ {
+ std::cout << "Please call me with: [./test_ceres_manager NI PRINT], where:" << std::endl;
+ std::cout << " - NI is the number of iterations (NI > 0)" << std::endl;
+ std::cout << " - WS is the window size (WS > 0)" << 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 window_size = (unsigned int) atoi(argv[2]);
+
+ // INITIALIZATION ============================================================================================
+ //init random generators
+ WolfScalar odom_std_factor = 0.5;
+ WolfScalar gps_std = 1;
+ std::default_random_engine generator(1);
+ std::normal_distribution<WolfScalar> distribution_odom(0.0, odom_std_factor); //odometry noise
+ std::normal_distribution<WolfScalar> distribution_gps(0.0, gps_std); //GPS noise
+
+ //init google log
+ //google::InitGoogleLogging(argv[0]);
+
+ // Faramotics stuff
+ Cpose3d viewPoint, devicePose, laser1Pose, laser2Pose, estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose;
+ vector < Cpose3d > devicePoses;
+ vector<float> scan1, scan2;
+ 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);
+ viewPoint.setPose(devicePose);
+ viewPoint.moveForward(10);
+ viewPoint.rt.setEuler(viewPoint.rt.head() + M_PI / 2, viewPoint.rt.pitch() + 30. * M_PI / 180., viewPoint.rt.roll());
+ viewPoint.moveForward(-15);
+ //glut initialization
+ 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::Vector3s odom_reading;
+ Eigen::Vector2s gps_fix_reading;
+ Eigen::VectorXs pose_odom(3); //current odometry integred pose
+ Eigen::VectorXs ground_truth(n_execution * 3); //all true poses
+ Eigen::VectorXs odom_trajectory(n_execution * 3); //open loop trajectory
+ Eigen::VectorXs mean_times = Eigen::VectorXs::Zero(7);
+ clock_t t1, t2;
+
+ // Wolf manager initialization
+ Eigen::Vector3s odom_pose = Eigen::Vector3s::Zero();
+ Eigen::Vector3s gps_pose = Eigen::Vector3s::Zero();
+ Eigen::Vector4s laser_1_pose, laser_2_pose; //xyz + theta
+ laser_1_pose << 1.2, 0, 0, 0; //laser 1
+ laser_2_pose << -1.2, 0, 0, M_PI; //laser 2
+ SensorOdom2D odom_sensor(new StateBlock(odom_pose.head(2)), new StateBlock(odom_pose.tail(1)), odom_std_factor, odom_std_factor);
+ SensorGPSFix gps_sensor(new StateBlock(gps_pose.head(2)), new StateBlock(gps_pose.tail(1)), gps_std);
+ SensorLaser2D laser_1_sensor(new StateBlock(laser_1_pose.head(2)), new StateBlock(laser_1_pose.tail(1)));
+ SensorLaser2D laser_2_sensor(new StateBlock(laser_2_pose.head(2)), new StateBlock(laser_2_pose.tail(1)));
+ laser_1_sensor.addProcessor(new ProcessorLaser2D());
+ laser_2_sensor.addProcessor(new ProcessorLaser2D());
+
+ // Initial pose
+ pose_odom << 2, 8, 0;
+ ground_truth.head(3) = pose_odom;
+ odom_trajectory.head(3) = pose_odom;
+
+ WolfManager* wolf_manager_ceres = new WolfManager(PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3);
+ WolfManager* wolf_manager_wolf = new WolfManager(PO_2D, &odom_sensor, pose_odom, Eigen::Matrix3s::Identity() * 0.01, window_size, 0.3);
+
+ // Ceres wrapper
+ ceres::Solver::Options ceres_options;
+ ceres_options.minimizer_type = ceres::TRUST_REGION; //ceres::TRUST_REGION;LINE_SEARCH
+ 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::TAKE_OWNERSHIP;//ceres::DO_NOT_TAKE_OWNERSHIP;
+ problem_options.local_parameterization_ownership = ceres::DO_NOT_TAKE_OWNERSHIP;
+ CeresManager* ceres_manager_ceres = new CeresManager(wolf_manager_ceres->getProblemPtr(), problem_options);
+ CeresManager* ceres_manager_wolf = new CeresManager(wolf_manager_wolf->getProblemPtr(), problem_options);
+ std::ofstream log_file, landmark_file; //output file
+
+ //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(2));
+ odom_reading(1) = 0;
+ 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(0) == 0 ? 1e-6 : odom_reading(0));
+ odom_reading(1) += distribution_odom(generator) * 1e-6;
+ odom_reading(2) += distribution_odom(generator) * (odom_reading(2) == 0 ? 1e-6 : odom_reading(2));
+
+ // odometry integration
+ pose_odom(0) = pose_odom(0) + odom_reading(0) * cos(pose_odom(2)) - odom_reading(1) * sin(pose_odom(2));
+ pose_odom(1) = pose_odom(1) + odom_reading(0) * sin(pose_odom(2)) + odom_reading(1) * cos(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);
+ gps_fix_reading(0) += distribution_gps(generator);
+ gps_fix_reading(1) += distribution_gps(generator);
+
+ //compute scans
+ scan1.clear();
+ scan2.clear();
+ // scan 1
+ laser1Pose.setPose(devicePose);
+ laser1Pose.moveForward(laser_1_pose(0));
+ myScanner->computeScan(laser1Pose, scan1);
+ // scan 2
+ laser2Pose.setPose(devicePose);
+ laser2Pose.moveForward(laser_2_pose(0));
+ laser2Pose.rt.setEuler(laser2Pose.rt.head() + M_PI, laser2Pose.rt.pitch(), laser2Pose.rt.roll());
+ myScanner->computeScan(laser2Pose, scan2);
+
+ mean_times(0) += ((double) clock() - t1) / CLOCKS_PER_SEC;
+
+ // ADD CAPTURES ---------------------------
+ std::cout << "ADD CAPTURES..." << std::endl;
+ t1 = clock();
+ // adding new sensor captures
+ wolf_manager_ceres->addCapture(new CaptureOdom2D(TimeStamp(),TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
+ wolf_manager_ceres->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
+ wolf_manager_ceres->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
+ wolf_manager_ceres->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
+ wolf_manager_wolf->addCapture(new CaptureOdom2D(TimeStamp(),TimeStamp(), &odom_sensor, odom_reading)); //, odom_std_factor * Eigen::MatrixXs::Identity(2,2)));
+ wolf_manager_wolf->addCapture(new CaptureGPSFix(TimeStamp(), &gps_sensor, gps_fix_reading, gps_std * Eigen::MatrixXs::Identity(3,3)));
+ wolf_manager_wolf->addCapture(new CaptureLaser2D(TimeStamp(), &laser_1_sensor, scan1));
+ wolf_manager_wolf->addCapture(new CaptureLaser2D(TimeStamp(), &laser_2_sensor, scan2));
+
+ // updating problem
+ wolf_manager_ceres->update();
+ wolf_manager_wolf->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_ceres->update();
+ ceres_manager_wolf->update(true);
+ mean_times(2) += ((double) clock() - t1) / CLOCKS_PER_SEC;
+
+ // SOLVE OPTIMIZATION ---------------------------
+ std::cout << "SOLVING..." << std::endl;
+ t1 = clock();
+ ceres::Solver::Summary summary_ceres = ceres_manager_ceres->solve(ceres_options);
+ ceres::Solver::Summary summary_wolf = ceres_manager_wolf->solve(ceres_options);
+ std::cout << "CERES AUTO DIFF" << std::endl;
+ std::cout << "Jacobian evaluation: " << summary_ceres.jacobian_evaluation_time_in_seconds << std::endl;
+ std::cout << "Total time: " << summary_ceres.total_time_in_seconds << std::endl;
+ std::cout << "WOLF AUTO DIFF" << std::endl;
+ std::cout << "Jacobian evaluation: " << summary_wolf.jacobian_evaluation_time_in_seconds << std::endl;
+ std::cout << "Total time: " << summary_wolf.total_time_in_seconds << std::endl;
+ mean_times(3) += ((double) clock() - t1) / CLOCKS_PER_SEC;
+
+ std::cout << "CERES AUTO DIFF solution:" << std::endl;
+ std::cout << wolf_manager_ceres->getVehiclePose().transpose() << std::endl;
+ std::cout << "WOLF AUTO DIFF solution:" << std::endl;
+ std::cout << wolf_manager_wolf->getVehiclePose().transpose() << std::endl;
+
+ // COMPUTE COVARIANCES ---------------------------
+ std::cout << "COMPUTING COVARIANCES..." << std::endl;
+ t1 = clock();
+ ceres_manager_ceres->computeCovariances(ALL_MARGINALS);
+ ceres_manager_wolf->computeCovariances(ALL_MARGINALS);
+ Eigen::MatrixXs marginal_ceres(3,3), marginal_wolf(3,3);
+ wolf_manager_ceres->getProblemPtr()->getCovarianceBlock(wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
+ wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
+ marginal_ceres, 0, 0);
+ wolf_manager_ceres->getProblemPtr()->getCovarianceBlock(wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
+ wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
+ marginal_ceres, 0, 2);
+ wolf_manager_ceres->getProblemPtr()->getCovarianceBlock(wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
+ wolf_manager_ceres->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
+ marginal_ceres, 2, 2);
+ wolf_manager_wolf->getProblemPtr()->getCovarianceBlock(wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
+ wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
+ marginal_wolf, 0, 0);
+ wolf_manager_wolf->getProblemPtr()->getCovarianceBlock(wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getPPtr(),
+ wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
+ marginal_wolf, 0, 2);
+ wolf_manager_wolf->getProblemPtr()->getCovarianceBlock(wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
+ wolf_manager_wolf->getProblemPtr()->getTrajectoryPtr()->getLastFramePtr()->getOPtr(),
+ marginal_wolf, 2, 2);
+ std::cout << "CERES AUTO DIFF covariance:" << std::endl;
+ std::cout << marginal_ceres << std::endl;
+ std::cout << "WOLF AUTO DIFF covariance:" << std::endl;
+ std::cout << marginal_wolf << std::endl;
+ 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(TimeStamp(), &laser_1_sensor, scan1);
+// 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(laser1Pose, 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);
+//
+// // draw localization and sensors
+// estimated_vehicle_pose.setPose(wolf_manager->getVehiclePose()(0), wolf_manager->getVehiclePose()(1), 0.2, wolf_manager->getVehiclePose()(2), 0, 0);
+// estimated_laser_1_pose.setPose(estimated_vehicle_pose);
+// estimated_laser_1_pose.moveForward(laser_1_pose(0));
+// estimated_laser_2_pose.setPose(estimated_vehicle_pose);
+// estimated_laser_2_pose.moveForward(laser_2_pose(0));
+// estimated_laser_2_pose.rt.setEuler(estimated_laser_2_pose.rt.head() + M_PI, estimated_laser_2_pose.rt.pitch(), estimated_laser_2_pose.rt.roll());
+// myRender->drawPoseAxisVector( { estimated_vehicle_pose, estimated_laser_1_pose, estimated_laser_2_pose });
+//
+// //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(laser1Pose, scan1, 180. * M_PI / 180., 90. * M_PI / 180.); //draw scan
+// myRender->render();
+// mean_times(5) += ((double) clock() - t1) / CLOCKS_PER_SEC;
+
+ // TIME MANAGEMENT ---------------------------
+ double dt = ((double) clock() - t2) / CLOCKS_PER_SEC;
+ mean_times(6) += dt;
+ if (dt < 0.1)
+ usleep(100000 - 1e6 * dt);
+
+// std::cout << "\nTree after step..." << std::endl;
+// wolf_manager->getProblemPtr()->print();
+ }
+
+ // 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->getProblemPtr()->print();
+
+// // 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++)
+// {
+// 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") + 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() << 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") + 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_ceres;
+ delete wolf_manager_wolf;
+ std::cout << "wolf deleted" << std::endl;
+ delete ceres_manager_ceres;
+ delete ceres_manager_wolf;
+ std::cout << "ceres_manager deleted" << std::endl;
+
+ std::cout << " ========= END ===========" << std::endl << std::endl;
+
+ //exit
+ return 0;
+}
--
GitLab