diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index b157ef55deaf606f4730ef5a08447b305af26af8..c15217fa4904eb2971551e08eed2794b4df28a8a 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -230,7 +230,7 @@ SET(HDRS_BASE ) SET(HDRS - capture_fix.h + capture_pose.h capture_gps_fix.h capture_imu.h capture_odom_2D.h @@ -242,9 +242,7 @@ SET(HDRS constraint_AHP.h constraint_epipolar.h constraint_imu.h - constraint_fix.h constraint_fix_bias.h - constraint_fix_3D.h constraint_gps_2D.h constraint_gps_pseudorange_3D.h constraint_gps_pseudorange_2D.h @@ -253,6 +251,8 @@ SET(HDRS constraint_odom_3D.h constraint_point_2D.h constraint_point_to_line_2D.h + constraint_pose_2D.h + constraint_pose_3D.h constraint_quaternion_absolute.h constraint_relative_2D_analytic.h feature_corner_2D.h @@ -322,11 +322,11 @@ SET(SRCS_BASE ) SET(SRCS - capture_fix.cpp capture_gps_fix.cpp capture_imu.cpp capture_odom_2D.cpp capture_odom_3D.cpp + capture_pose.cpp capture_void.cpp feature_corner_2D.cpp feature_gps_fix.cpp diff --git a/src/capture_fix.cpp b/src/capture_pose.cpp similarity index 75% rename from src/capture_fix.cpp rename to src/capture_pose.cpp index 7b3c9106f219b4597ffa0b698976cde7c13a09de..dda2327f3e8a7c6882e865a43db9d15d40c52061 100644 --- a/src/capture_fix.cpp +++ b/src/capture_pose.cpp @@ -1,8 +1,8 @@ -#include "capture_fix.h" +#include "capture_pose.h" namespace wolf{ -CaptureFix::CaptureFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) : +CapturePose::CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) : CaptureBase("FIX", _ts, _sensor_ptr), data_(_data), data_covariance_(_data_covariance) @@ -10,12 +10,12 @@ CaptureFix::CaptureFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Ei // } -CaptureFix::~CaptureFix() +CapturePose::~CapturePose() { // } -void CaptureFix::emplaceFeatureAndConstraint() +void CapturePose::emplaceFeatureAndConstraint() { // Emplace feature FeatureFixPtr feature_fix = std::make_shared<FeatureFix>(data_,data_covariance_); diff --git a/src/capture_fix.h b/src/capture_pose.h similarity index 56% rename from src/capture_fix.h rename to src/capture_pose.h index f9e66608315f8c948d6a942924f7031d930b5865..2a843375fe65ed15ab272e25594a8d1272184a10 100644 --- a/src/capture_fix.h +++ b/src/capture_pose.h @@ -1,5 +1,5 @@ -#ifndef CAPTURE_FIX_H_ -#define CAPTURE_FIX_H_ +#ifndef CAPTURE_POSE_H_ +#define CAPTURE_POSE_H_ //Wolf includes #include "capture_base.h" @@ -12,10 +12,10 @@ namespace wolf { -WOLF_PTR_TYPEDEFS(CaptureFix); +WOLF_PTR_TYPEDEFS(CapturePose); -//class CaptureFix -class CaptureFix : public CaptureBase +//class CapturePose +class CapturePose : public CaptureBase { protected: Eigen::VectorXs data_; ///< Raw data. @@ -23,9 +23,9 @@ class CaptureFix : public CaptureBase public: - CaptureFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance); + CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance); - virtual ~CaptureFix(); + virtual ~CapturePose(); virtual void emplaceFeatureAndConstraint(); diff --git a/src/examples/test_2_lasers_offline.cpp b/src/examples/test_2_lasers_offline.cpp index 6c38c19cbe3baeb13873766f9f8d77d1642974e2..d0b262886d00266461bafb2d34b6a0d6e443fd16 100644 --- a/src/examples/test_2_lasers_offline.cpp +++ b/src/examples/test_2_lasers_offline.cpp @@ -22,7 +22,6 @@ #include "../sensor_laser_2D.h" #include "../sensor_odom_2D.h" #include "../sensor_gps_fix.h" -#include "../capture_fix.h" #include "../ceres_wrapper/ceres_manager.h" // laserscanutils @@ -32,6 +31,7 @@ //C includes for sleep, time and main args #include "unistd.h" +#include "../capture_pose.h" //faramotics includes #include "faramotics/dynamicSceneRender.h" #include "faramotics/rangeScan2D.h" diff --git a/src/examples/test_ceres_2_lasers.cpp b/src/examples/test_ceres_2_lasers.cpp index 1bc45cf0b426419809ebe442f82801b616d1452c..0498bca76cd95eaf464e8d265b3b2e46d8e6fc76 100644 --- a/src/examples/test_ceres_2_lasers.cpp +++ b/src/examples/test_ceres_2_lasers.cpp @@ -22,7 +22,6 @@ #include "sensor_laser_2D.h" #include "sensor_odom_2D.h" #include "sensor_gps_fix.h" -#include "capture_fix.h" #include "ceres_wrapper/ceres_manager.h" // laserscanutils @@ -32,6 +31,7 @@ //C includes for sleep, time and main args #include "unistd.h" +#include "../capture_pose.h" //faramotics includes #include "faramotics/dynamicSceneRender.h" #include "faramotics/rangeScan2D.h" @@ -251,7 +251,7 @@ int main(int argc, char** argv) FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts); // Prior covariance - CaptureFix* initial_covariance = new CaptureFix(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1); + CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1); origin_frame->addCapture(initial_covariance); initial_covariance->process(); diff --git a/src/examples/test_ceres_2_lasers_polylines.cpp b/src/examples/test_ceres_2_lasers_polylines.cpp index 9c5f592ec5728d999f66cc688f73739d080eef86..ad821fffc9d3d426e0ce0a1f5a74726d50114c67 100644 --- a/src/examples/test_ceres_2_lasers_polylines.cpp +++ b/src/examples/test_ceres_2_lasers_polylines.cpp @@ -22,7 +22,6 @@ #include "sensor_laser_2D.h" #include "sensor_odom_2D.h" #include "sensor_gps_fix.h" -#include "capture_fix.h" #include "ceres_wrapper/ceres_manager.h" // laserscanutils @@ -32,6 +31,7 @@ //C includes for sleep, time and main args #include "unistd.h" +#include "../capture_pose.h" //faramotics includes #include "faramotics/dynamicSceneRender.h" #include "faramotics/rangeScan2D.h" @@ -258,7 +258,7 @@ int main(int argc, char** argv) FrameBasePtr origin_frame = problem.createFrame(KEY_FRAME, ground_truth_pose, ts); // Prior covariance - CaptureFix* initial_covariance = new CaptureFix(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1); + CapturePose* initial_covariance = new CapturePose(ts, gps_sensor, ground_truth_pose, Eigen::Matrix3s::Identity() * 0.1); origin_frame->addCapture(initial_covariance); initial_covariance->process(); diff --git a/src/examples/test_constraint_imu.cpp b/src/examples/test_constraint_imu.cpp index 4f85de71343c2ca545f86fcf600d7b8fd65d8b35..b802e384efca7693df955196cb6a24035baa14b3 100644 --- a/src/examples/test_constraint_imu.cpp +++ b/src/examples/test_constraint_imu.cpp @@ -1,4 +1,5 @@ //Wolf +#include "../capture_pose.h" #include "wolf.h" #include "problem.h" #include "sensor_imu.h" @@ -7,7 +8,6 @@ #include "state_block.h" #include "state_quaternion.h" #include "processor_imu.h" -#include "capture_fix.h" #include "ceres_wrapper/ceres_manager.h" //#define DEBUG_RESULTS @@ -249,7 +249,7 @@ int main(int argc, char** argv) //FrameBasePtr first_frame = wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().front(); wolf_problem_ptr_->getProcessorMotionPtr()->setOrigin(wolf_problem_ptr_->getTrajectoryPtr()->getFrameList().front()); //SensorBasePtr sensorbase = std::make_shared<SensorBase>("ABSOLUTE POSE", nullptr, nullptr, nullptr, 0); - //CaptureFixPtr initial_covariance = std::make_shared<CaptureFix>(TimeStamp(0), sensorbase, first_frame->getState().head(7), Eigen::Matrix6s::Identity() * 0.01); + //CapturePosePtr initial_covariance = std::make_shared<CaptureFix>(TimeStamp(0), sensorbase, first_frame->getState().head(7), Eigen::Matrix6s::Identity() * 0.01); //first_frame->addCapture(initial_covariance); //initial_covariance->process(); //std::cout << "initial covariance: constraint " << initial_covariance->getFeatureList().front()->getConstrainedByList().front()->id() << std::endl << initial_covariance->getFeatureList().front()->getMeasurementCovariance() << std::endl; diff --git a/src/examples/test_processor_image_landmark.cpp b/src/examples/test_processor_image_landmark.cpp index 9b6bd7dcc04d545f85a517c333e9ffd710474dc2..1fb2d47fe13d4ae4f140bac17e181d5998aa1599 100644 --- a/src/examples/test_processor_image_landmark.cpp +++ b/src/examples/test_processor_image_landmark.cpp @@ -8,12 +8,12 @@ //std #include <iostream> +#include "../capture_pose.h" //Wolf #include "wolf.h" #include "problem.h" #include "state_block.h" #include "processor_image_landmark.h" -#include "capture_fix.h" #include "processor_odom_3D.h" #include "sensor_odom_3D.h" #include "ceres_wrapper/ceres_manager.h" diff --git a/src/problem.cpp b/src/problem.cpp index 6518979d518f3a52d6b7ca6d2f20b4ff7e175ad4..e801e145080c655d08a7db25feb8c2649881b9d3 100644 --- a/src/problem.cpp +++ b/src/problem.cpp @@ -9,12 +9,12 @@ #include "processor_tracker.h" #include "sensor_base.h" #include "sensor_gps.h" -#include "capture_fix.h" #include "factory.h" #include "sensor_factory.h" #include "processor_factory.h" #include <algorithm> +#include "capture_pose.h" namespace wolf { @@ -637,11 +637,11 @@ FrameBasePtr Problem::setPrior(const Eigen::VectorXs& _prior_state, const Eigen: // create origin capture with the given state as data // Capture fix only takes 3D position and Quaternion orientation - CaptureFixPtr init_capture; + CapturePosePtr init_capture; if (trajectory_ptr_->getFrameStructure() == "POV 3D") - init_capture = std::make_shared<CaptureFix>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6)); + init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state.head(7), _prior_cov.topLeftCorner(6,6)); else - init_capture = std::make_shared<CaptureFix>(_ts, nullptr, _prior_state, _prior_cov); + init_capture = std::make_shared<CapturePose>(_ts, nullptr, _prior_state, _prior_cov); origin_frame_ptr->addCapture(init_capture); diff --git a/src/test/gtest_odom_2D.cpp b/src/test/gtest_odom_2D.cpp index 8af6e20fb765678039ff1b0697f2ea0c5886e568..76bd0e6db0a116bdf304bf23fcbc5dbc8c976f9a 100644 --- a/src/test/gtest_odom_2D.cpp +++ b/src/test/gtest_odom_2D.cpp @@ -13,7 +13,6 @@ // Wolf includes #include "../sensor_odom_2D.h" -#include "../capture_fix.h" #include "../state_block.h" #include "../wolf.h" #include "../ceres_wrapper/ceres_manager.h" @@ -27,6 +26,7 @@ // General includes #include <iostream> #include <iomanip> // std::setprecision +#include "../capture_pose.h" using namespace wolf; using namespace Eigen;