diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
new file mode 100644
index 0000000000000000000000000000000000000000..29ae1e9fadef99e72393ea8581f78d4894170d9b
--- /dev/null
+++ b/.gitlab-ci.yml
@@ -0,0 +1,83 @@
+image: segaleran/ceres
+
+cache:
+ key: "$CI_COMMIT_REF_NAME"
+ untracked: true
+ paths:
+ - build
+ - bin
+ - lib
+
+before_script:
+ - ls
+ - apt-get update
+ - apt-get install -y build-essential cmake
+
+# SPDLOG
+# - apt-get install -y libspdlog-dev
+ - if [ -d spdlog ]; then
+ - echo "directory exists"
+ - if [ "$(ls -A ./spdlog)" ]; then
+ - echo "directory not empty"
+ - cd spdlog
+ - git pull
+ - else
+ - echo "directory empty"
+ - git clone https://github.com/gabime/spdlog.git
+ - cd spdlog
+ - fi
+ - else
+ - echo "directory inexistent"
+ - git clone https://github.com/gabime/spdlog.git
+ - cd spdlog
+ - fi
+ - mkdir -pv build
+ - cd build
+ - ls
+ - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DSPDLOG_BUILD_TESTING=OFF ..
+ - make install
+ - cd ../..
+
+# YAML
+# - apt-get install -y libyaml-cpp-dev
+ - if [ -d yaml-cpp ]; then
+ - echo "directory exists"
+ - if [ "$(ls -A ./yaml-cpp)" ]; then
+ - echo "directory not empty"
+ - cd yaml-cpp
+ - git pull
+ - else
+ - echo "directory empty"
+ - git clone https://github.com/jbeder/yaml-cpp.git
+ - cd yaml-cpp
+ - fi
+ - else
+ - echo "directory inexistent"
+ - git clone https://github.com/jbeder/yaml-cpp.git
+ - cd yaml-cpp
+ - fi
+ - mkdir -pv build
+ - cd build
+ - ls
+ - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DYAML_CPP_BUILD_TESTS=OFF ..
+ - make install
+ - cd ../..
+
+wolf_build:
+ stage: build
+# only:
+# - renameFixPose
+ script:
+ - mkdir -pv build
+ - cd build
+ - ls # we can check whether the directory was already full
+ - cmake -DCMAKE_BUILD_TYPE=release ..
+ - make
+
+wolf_test:
+ stage: test
+# only:
+# - renameFixPose
+ script:
+ - cd build
+ - ctest
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index b157ef55deaf606f4730ef5a08447b305af26af8..22d70f19a0deea4dc27652bf00b1428140cd2270 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,15 +251,17 @@ 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
feature_gps_fix.h
feature_gps_pseudorange.h
feature_imu.h
- feature_fix.h
feature_odom_2D.h
feature_polyline_2D.h
+ feature_pose.h
landmark_corner_2D.h
landmark_container.h
landmark_line_2D.h
@@ -322,19 +322,19 @@ 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
feature_gps_pseudorange.cpp
feature_imu.cpp
- feature_fix.cpp
feature_odom_2D.cpp
feature_polyline_2D.cpp
+ feature_pose.cpp
landmark_corner_2D.cpp
landmark_container.cpp
landmark_line_2D.cpp
diff --git a/src/capture_fix.cpp b/src/capture_fix.cpp
deleted file mode 100644
index e502f4c42b56ff2745d9fa7f2fc174168c40cb71..0000000000000000000000000000000000000000
--- a/src/capture_fix.cpp
+++ /dev/null
@@ -1,34 +0,0 @@
-#include "capture_fix.h"
-
-namespace wolf{
-
-CaptureFix::CaptureFix(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)
-{
- //
-}
-
-CaptureFix::~CaptureFix()
-{
- //
-}
-
-void CaptureFix::emplaceFeatureAndConstraint()
-{
- // Emplace feature
- FeatureFixPtr feature_fix = std::make_shared<FeatureFix>(data_,data_covariance_);
- addFeature(feature_fix);
-
- // Emplace constraint
- if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3 )
- feature_fix->addConstraint(std::make_shared<ConstraintFix>(feature_fix));
- else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6 )
- feature_fix->addConstraint(std::make_shared<ConstraintFix3D>(feature_fix));
- else
- throw std::runtime_error("Wrong data size in CaptureFix. Use 3 for 2D. Use 7 for 3D.");
-}
-
-
-} // namespace wolf
diff --git a/src/capture_fix.h b/src/capture_fix.h
deleted file mode 100644
index b399c3288d5846de48d4a6faa34faeb7031f913d..0000000000000000000000000000000000000000
--- a/src/capture_fix.h
+++ /dev/null
@@ -1,35 +0,0 @@
-#ifndef CAPTURE_FIX_H_
-#define CAPTURE_FIX_H_
-
-//Wolf includes
-#include "capture_base.h"
-#include "feature_fix.h"
-#include "constraint_fix.h"
-#include "constraint_fix_3D.h"
-
-//std includes
-//
-
-namespace wolf {
-
-WOLF_PTR_TYPEDEFS(CaptureFix);
-
-//class CaptureFix
-class CaptureFix : public CaptureBase
-{
- protected:
- Eigen::VectorXs data_; ///< Raw data.
- Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
-
- public:
-
- CaptureFix(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
-
- virtual ~CaptureFix();
-
- virtual void emplaceFeatureAndConstraint();
-
-};
-
-} //namespace wolf
-#endif
diff --git a/src/capture_pose.cpp b/src/capture_pose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..c8422aa32a8ec4183c5c48cdcb74002437202709
--- /dev/null
+++ b/src/capture_pose.cpp
@@ -0,0 +1,34 @@
+#include "capture_pose.h"
+
+namespace wolf{
+
+CapturePose::CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
+ CaptureBase("POSE", _ts, _sensor_ptr),
+ data_(_data),
+ data_covariance_(_data_covariance)
+{
+ //
+}
+
+CapturePose::~CapturePose()
+{
+ //
+}
+
+void CapturePose::emplaceFeatureAndConstraint()
+{
+ // Emplace feature
+ FeaturePosePtr feature_pose = std::make_shared<FeaturePose>(data_,data_covariance_);
+ addFeature(feature_pose);
+
+ // Emplace constraint
+ if (data_.size() == 3 && data_covariance_.rows() == 3 && data_covariance_.cols() == 3 )
+ feature_pose->addConstraint(std::make_shared<ConstraintPose2D>(feature_pose));
+ else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6 )
+ feature_pose->addConstraint(std::make_shared<ConstraintPose3D>(feature_pose));
+ else
+ throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2D. Use 7 for 3D.");
+}
+
+
+} // namespace wolf
diff --git a/src/capture_pose.h b/src/capture_pose.h
new file mode 100644
index 0000000000000000000000000000000000000000..9d47d39c98976458bff882e1770b7f03ee1357a5
--- /dev/null
+++ b/src/capture_pose.h
@@ -0,0 +1,35 @@
+#ifndef CAPTURE_POSE_H_
+#define CAPTURE_POSE_H_
+
+//Wolf includes
+#include "capture_base.h"
+#include "constraint_pose_2D.h"
+#include "constraint_pose_3D.h"
+#include "feature_pose.h"
+
+//std includes
+//
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(CapturePose);
+
+//class CapturePose
+class CapturePose : public CaptureBase
+{
+ protected:
+ Eigen::VectorXs data_; ///< Raw data.
+ Eigen::MatrixXs data_covariance_; ///< Noise of the capture.
+
+ public:
+
+ CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance);
+
+ virtual ~CapturePose();
+
+ virtual void emplaceFeatureAndConstraint();
+
+};
+
+} //namespace wolf
+#endif
diff --git a/src/constraint_fix.h b/src/constraint_pose_2D.h
similarity index 66%
rename from src/constraint_fix.h
rename to src/constraint_pose_2D.h
index eb9221365a30b87b864577e2a31cc60e14126cf5..c971f72f9bd3f06ecda26113cad9f5456c50b564 100644
--- a/src/constraint_fix.h
+++ b/src/constraint_pose_2D.h
@@ -1,6 +1,6 @@
-#ifndef CONSTRAINT_FIX_H_
-#define CONSTRAINT_FIX_H_
+#ifndef CONSTRAINT_POSE_2D_H_
+#define CONSTRAINT_POSE_2D_H_
//Wolf includes
#include "constraint_autodiff.h"
@@ -12,20 +12,20 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintFix);
+WOLF_PTR_TYPEDEFS(ConstraintPose2D);
//class
-class ConstraintFix: public ConstraintAutodiff<ConstraintFix,3,2,1>
+class ConstraintPose2D: public ConstraintAutodiff<ConstraintPose2D,3,2,1>
{
public:
- ConstraintFix(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintFix, 3, 2, 1>(CTR_FIX, nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+ ConstraintPose2D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintAutodiff<ConstraintPose2D, 3, 2, 1>(CTR_POSE_2D, nullptr, nullptr, nullptr, nullptr, nullptr,_apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
{
- setType("FIX");
-// std::cout << "created ConstraintFix " << std::endl;
+ setType("POSE 2D");
+// std::cout << "created ConstraintPose2D " << std::endl;
}
- virtual ~ConstraintFix() = default;
+ virtual ~ConstraintPose2D() = default;
template<typename T>
bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
@@ -43,7 +43,7 @@ class ConstraintFix: public ConstraintAutodiff<ConstraintFix,3,2,1>
};
template<typename T>
-inline bool ConstraintFix::operator ()(const T* const _p, const T* const _o, T* _residuals) const
+inline bool ConstraintPose2D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
{
// measurement
Eigen::Matrix<T,3,1> meas = getMeasurement().cast<T>();
@@ -74,8 +74,8 @@ inline bool ConstraintFix::operator ()(const T* const _p, const T* const _o, T*
// J.row(2) = ((Jet<Scalar, 3>)(res(2))).v;
// if (sizeof(er(0)) != sizeof(double))
// {
-// std::cout << "ConstraintFix::Jacobian(c" << id() << ") = \n " << J << std::endl;
-// std::cout << "ConstraintFix::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "ConstraintPose2D::Jacobian(c" << id() << ") = \n " << J << std::endl;
+// std::cout << "ConstraintPose2D::Weighted Jacobian(c" << id() << ") = \n " << J << std::endl;
// std::cout << "Sqrt Info(c" << id() << ") = \n " << getMeasurementSquareRootInformationUpper() << std::endl;
// }
////////////////////////////////////////////////////////
diff --git a/src/constraint_fix_3D.h b/src/constraint_pose_3D.h
similarity index 61%
rename from src/constraint_fix_3D.h
rename to src/constraint_pose_3D.h
index c9c7b8b59965e2290f37edf5925d1f6077fabcae..d0132756a8b2c6617e4114799d35126df758501d 100644
--- a/src/constraint_fix_3D.h
+++ b/src/constraint_pose_3D.h
@@ -1,6 +1,6 @@
-#ifndef CONSTRAINT_FIX_3D_H_
-#define CONSTRAINT_FIX_3D_H_
+#ifndef CONSTRAINT_POSE_3D_H_
+#define CONSTRAINT_POSE_3D_H_
//Wolf includes
#include "constraint_autodiff.h"
@@ -10,20 +10,20 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(ConstraintFix3D);
+WOLF_PTR_TYPEDEFS(ConstraintPose3D);
//class
-class ConstraintFix3D: public ConstraintAutodiff<ConstraintFix3D,6,3,4>
+class ConstraintPose3D: public ConstraintAutodiff<ConstraintPose3D,6,3,4>
{
public:
- ConstraintFix3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
- ConstraintAutodiff<ConstraintFix3D,6,3,4>(CTR_FIX_3D, nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
+ ConstraintPose3D(FeatureBasePtr _ftr_ptr, bool _apply_loss_function = false, ConstraintStatus _status = CTR_ACTIVE) :
+ ConstraintAutodiff<ConstraintPose3D,6,3,4>(CTR_POSE_3D, nullptr, nullptr, nullptr, nullptr, nullptr, _apply_loss_function, _status, _ftr_ptr->getFramePtr()->getPPtr(), _ftr_ptr->getFramePtr()->getOPtr())
{
setType("FIX3D");
}
- virtual ~ConstraintFix3D() = default;
+ virtual ~ConstraintPose3D() = default;
template<typename T>
bool operator ()(const T* const _p, const T* const _o, T* _residuals) const;
@@ -36,7 +36,7 @@ class ConstraintFix3D: public ConstraintAutodiff<ConstraintFix3D,6,3,4>
};
template<typename T>
-inline bool ConstraintFix3D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
+inline bool ConstraintPose3D::operator ()(const T* const _p, const T* const _o, T* _residuals) const
{
// states
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_analytic_odom_constraint.cpp b/src/examples/test_analytic_odom_constraint.cpp
index 93fb031c5d8d3efa82e5d0cb1dd14fb02c18c292..029b0280bc33d469361960af15a18432a8f4fa55 100644
--- a/src/examples/test_analytic_odom_constraint.cpp
+++ b/src/examples/test_analytic_odom_constraint.cpp
@@ -238,7 +238,7 @@ int main(int argc, char** argv)
bNum.clear();
// add capture, feature and constraint to problem
- FeatureBasePtr feature_ptr_autodiff = new FeatureBase("FIX", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_autodiff = new FeatureBase("POSE", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_autodiff = new CaptureVoid(TimeStamp(0), sensor);
assert(index_2_frame_ptr_autodiff.find(edge_old) != index_2_frame_ptr_autodiff.end() && "edge from vertex not added!");
FrameBasePtr frame_old_ptr_autodiff = index_2_frame_ptr_autodiff[edge_old];
@@ -251,7 +251,7 @@ int main(int argc, char** argv)
//std::cout << "Added autodiff edge! " << constraint_ptr_autodiff->id() << " from vertex " << constraint_ptr_autodiff->getCapturePtr()->getFramePtr()->id() << " to " << constraint_ptr_autodiff->getFrameOtherPtr()->id() << std::endl;
// add capture, feature and constraint to problem
- FeatureBasePtr feature_ptr_analytic = new FeatureBase("FIX", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_analytic = new FeatureBase("POSE", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_analytic = new CaptureVoid(TimeStamp(0), sensor);
assert(index_2_frame_ptr_analytic.find(edge_old) != index_2_frame_ptr_analytic.end() && "edge from vertex not added!");
FrameBasePtr frame_old_ptr_analytic = index_2_frame_ptr_analytic[edge_old];
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_imuDock.cpp b/src/examples/test_imuDock.cpp
index 3c7ce840356b94eae7e357003912ea8ffc301285..cc38459099d7b35ee5192b57a9e225312f4d82df 100644
--- a/src/examples/test_imuDock.cpp
+++ b/src/examples/test_imuDock.cpp
@@ -14,12 +14,12 @@
#include "processor_odom_3D.h"
//Constraints headers
-#include "constraint_fix_3D.h"
#include "constraint_fix_bias.h"
//std
#include <iostream>
#include <fstream>
+#include "../constraint_pose_3D.h"
#define OUTPUT_RESULTS
//#define ADD_KF3
@@ -183,7 +183,7 @@ int main(int argc, char** argv)
featureFix_cov(5,5) = pow( .01 , 2); // yaw variance
CaptureBasePtr cap_fix = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.head(7), 7, 6, nullptr));
FeatureBasePtr featureFix = cap_fix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", problem_origin.head(7), featureFix_cov));
- ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintFix3D>(featureFix->addConstraint(std::make_shared<ConstraintFix3D>(featureFix)));
+ ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintPose3D>(featureFix->addConstraint(std::make_shared<ConstraintPose3D>(featureFix)));
Eigen::MatrixXs featureFixBias_cov(6,6);
featureFixBias_cov = Eigen::MatrixXs::Identity(6,6);
diff --git a/src/examples/test_imuDock_autoKFs.cpp b/src/examples/test_imuDock_autoKFs.cpp
index a74771426469e4a0dd97131d55dffb5cd7e9b9a6..660c6a240c3a491e7687241316a29f5349e8f405 100644
--- a/src/examples/test_imuDock_autoKFs.cpp
+++ b/src/examples/test_imuDock_autoKFs.cpp
@@ -14,12 +14,12 @@
#include "processor_odom_3D.h"
//Constraints headers
-#include "constraint_fix_3D.h"
#include "constraint_fix_bias.h"
//std
#include <iostream>
#include <fstream>
+#include "../constraint_pose_3D.h"
#define OUTPUT_RESULTS
//#define AUTO_KFS
@@ -198,7 +198,7 @@ int main(int argc, char** argv)
featureFix_cov(5,5) = pow( .001 , 2); // yaw variance
CaptureBasePtr cap_fix = KF1->addCapture(std::make_shared<CaptureMotion>(0, nullptr, problem_origin.head(7), 7, 6, nullptr));
FeatureBasePtr featureFix = cap_fix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", problem_origin.head(7), featureFix_cov));
- ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintFix3D>(featureFix->addConstraint(std::make_shared<ConstraintFix3D>(featureFix)));
+ ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintPose3D>(featureFix->addConstraint(std::make_shared<ConstraintPose3D>(featureFix)));
Eigen::MatrixXs featureFixBias_cov(6,6);
featureFixBias_cov = Eigen::MatrixXs::Identity(6,6);
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/examples/test_wolf_autodiffwrapper.cpp b/src/examples/test_wolf_autodiffwrapper.cpp
index 318f085aba78729badec8e4a6117f12ad3bb547d..5728739a81573f2e99853077d5452ed8b85dd755 100644
--- a/src/examples/test_wolf_autodiffwrapper.cpp
+++ b/src/examples/test_wolf_autodiffwrapper.cpp
@@ -225,8 +225,8 @@ int main(int argc, char** argv)
bNum.clear();
// add capture, feature and constraint to problem
- FeatureBasePtr feature_ptr_ceres_diff = new FeatureBase("FIX", edge_vector, edge_information.inverse());
- FeatureBasePtr feature_ptr_wolf_diff = new FeatureBase("FIX", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_ceres_diff = new FeatureBase("POSE", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_wolf_diff = new FeatureBase("POSE", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_ceres_diff = new CaptureVoid(TimeStamp(0), sensor);
CaptureVoid* capture_ptr_wolf_diff = new CaptureVoid(TimeStamp(0), sensor);
assert(index_2_frame_ptr_ceres_diff.find(edge_old) != index_2_frame_ptr_ceres_diff.end() && "edge from vertex not added!");
diff --git a/src/examples/test_wolf_imported_graph.cpp b/src/examples/test_wolf_imported_graph.cpp
index baa9cc7ce5c6397a19dd0f27e706c00ecd2929f9..fc8129dc55d1e0152cb621bdde48be0df6368cd3 100644
--- a/src/examples/test_wolf_imported_graph.cpp
+++ b/src/examples/test_wolf_imported_graph.cpp
@@ -246,8 +246,8 @@ int main(int argc, char** argv)
bNum.clear();
// add capture, feature and constraint to problem
- FeatureBasePtr feature_ptr_full = new FeatureBase("FIX", edge_vector, edge_information.inverse());
- FeatureBasePtr feature_ptr_prun = new FeatureBase("FIX", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_full = new FeatureBase("POSE", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_prun = new FeatureBase("POSE", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_full = new CaptureVoid(TimeStamp(0), sensor);
CaptureVoid* capture_ptr_prun = new CaptureVoid(TimeStamp(0), sensor);
assert(index_2_frame_ptr_full.find(edge_old) != index_2_frame_ptr_full.end() && "edge from vertex not added!");
diff --git a/src/examples/test_wolf_prunning.cpp b/src/examples/test_wolf_prunning.cpp
index 92fcc6d98d46764cb8cfd0a89c128bfb0e4cf14a..3191326c0d50a5c26b3d9ead26d4a86beca9ebb6 100644
--- a/src/examples/test_wolf_prunning.cpp
+++ b/src/examples/test_wolf_prunning.cpp
@@ -263,8 +263,8 @@ int main(int argc, char** argv)
//std::cout << "Adding edge... " << std::endl;
// add capture, feature and constraint to problem
- FeatureBasePtr feature_ptr_full = new FeatureBase("FIX", edge_vector, edge_information.inverse());
- FeatureBasePtr feature_ptr_prun = new FeatureBase("FIX", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_full = new FeatureBase("POSE", edge_vector, edge_information.inverse());
+ FeatureBasePtr feature_ptr_prun = new FeatureBase("POSE", edge_vector, edge_information.inverse());
CaptureVoid* capture_ptr_full = new CaptureVoid(TimeStamp(0), sensor);
CaptureVoid* capture_ptr_prun = new CaptureVoid(TimeStamp(0), sensor);
assert(index_2_frame_ptr_full.find(edge_old) != index_2_frame_ptr_full.end() && "edge from vertex not added!");
diff --git a/src/feature_fix.cpp b/src/feature_fix.cpp
deleted file mode 100644
index 5497695a41759bb15b2cf00a1dc679f692f8a325..0000000000000000000000000000000000000000
--- a/src/feature_fix.cpp
+++ /dev/null
@@ -1,17 +0,0 @@
-#include "feature_fix.h"
-
-
-namespace wolf {
-
-FeatureFix::FeatureFix(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
- FeatureBase("FIX", _measurement, _meas_covariance)
-{
- //
-}
-
-FeatureFix::~FeatureFix()
-{
- //
-}
-
-} // namespace wolf
diff --git a/src/feature_pose.cpp b/src/feature_pose.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..35193d2b07b2207721c23da3aa776171e42a0d2b
--- /dev/null
+++ b/src/feature_pose.cpp
@@ -0,0 +1,17 @@
+#include "feature_pose.h"
+
+
+namespace wolf {
+
+FeaturePose::FeaturePose(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance) :
+ FeatureBase("POSE", _measurement, _meas_covariance)
+{
+ //
+}
+
+FeaturePose::~FeaturePose()
+{
+ //
+}
+
+} // namespace wolf
diff --git a/src/feature_fix.h b/src/feature_pose.h
similarity index 55%
rename from src/feature_fix.h
rename to src/feature_pose.h
index d7649bcc3bfb163c0b3a7b2bb46c2214e8c0d50a..f4d7ba0612f8e313d1be4e54618ab540d6a78007 100644
--- a/src/feature_fix.h
+++ b/src/feature_pose.h
@@ -1,5 +1,5 @@
-#ifndef FEATURE_FIX_H_
-#define FEATURE_FIX_H_
+#ifndef FEATURE_POSE_H_
+#define FEATURE_POSE_H_
//Wolf includes
@@ -10,10 +10,10 @@
namespace wolf {
-WOLF_PTR_TYPEDEFS(FeatureFix);
+WOLF_PTR_TYPEDEFS(FeaturePose);
-//class FeatureFix
-class FeatureFix : public FeatureBase
+//class FeaturePose
+class FeaturePose : public FeatureBase
{
public:
@@ -23,8 +23,8 @@ class FeatureFix : public FeatureBase
* \param _meas_covariance the noise of the measurement
*
*/
- FeatureFix(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
- virtual ~FeatureFix();
+ FeaturePose(const Eigen::VectorXs& _measurement, const Eigen::MatrixXs& _meas_covariance);
+ virtual ~FeaturePose();
};
} // namespace wolf
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/solver/qr_solver.h b/src/solver/qr_solver.h
index d8d097fb7bcd44807c062107a58c1655bc7e504c..493ee2dc8980b7236a762b47d0353b9b54eae0f6 100644
--- a/src/solver/qr_solver.h
+++ b/src/solver/qr_solver.h
@@ -15,9 +15,6 @@
//Wolf includes
#include "state_block.h"
#include "../constraint_sparse.h"
-#include "../constraint_fix.h"
-//#include "../constraint_gps_2D.h"
-//#include "../constraint_gps_pseudorange_2D.h"
#include "../constraint_odom_2D.h"
#include "../constraint_corner_2D.h"
#include "../constraint_container.h"
@@ -31,6 +28,7 @@
#include <eigen3/Eigen/OrderingMethods>
#include <eigen3/Eigen/SparseQR>
#include <Eigen/StdVector>
+#include "../constraint_pose_2D.h"
namespace wolf
{
diff --git a/src/test/CMakeLists.txt b/src/test/CMakeLists.txt
index 6014f353067b002c80144ee66608f3cf062f0fe5..579035b175fb36109b234201ac747351831313c5 100644
--- a/src/test/CMakeLists.txt
+++ b/src/test/CMakeLists.txt
@@ -94,18 +94,18 @@ target_link_libraries(gtest_trajectory ${PROJECT_NAME})
wolf_add_gtest(gtest_constraint_absolute gtest_constraint_absolute.cpp)
target_link_libraries(gtest_constraint_absolute ${PROJECT_NAME})
-# ConstraintFix3D class test
-wolf_add_gtest(gtest_constraint_fix_3D gtest_constraint_fix_3D.cpp)
-target_link_libraries(gtest_constraint_fix_3D ${PROJECT_NAME})
-
-# ConstraintFix class test
-wolf_add_gtest(gtest_constraint_fix gtest_constraint_fix.cpp)
-target_link_libraries(gtest_constraint_fix ${PROJECT_NAME})
-
# ConstraintOdom3D class test
wolf_add_gtest(gtest_constraint_odom_3D gtest_constraint_odom_3D.cpp)
target_link_libraries(gtest_constraint_odom_3D ${PROJECT_NAME})
+# ConstraintPose2D class test
+wolf_add_gtest(gtest_constraint_pose_2D gtest_constraint_pose_2D.cpp)
+target_link_libraries(gtest_constraint_pose_2D ${PROJECT_NAME})
+
+# ConstraintPose3D class test
+wolf_add_gtest(gtest_constraint_pose_3D gtest_constraint_pose_3D.cpp)
+target_link_libraries(gtest_constraint_pose_3D ${PROJECT_NAME})
+
# FeatureIMU test
wolf_add_gtest(gtest_feature_imu gtest_feature_imu.cpp)
target_link_libraries(gtest_feature_imu ${PROJECT_NAME})
diff --git a/src/test/gtest_constraint_absolute.cpp b/src/test/gtest_constraint_absolute.cpp
index 0f190fef2ec73affcbdc37ff70a2c730c2c23c5e..52adcd099403218395990b69ed857da62259d507 100644
--- a/src/test/gtest_constraint_absolute.cpp
+++ b/src/test/gtest_constraint_absolute.cpp
@@ -51,7 +51,7 @@ CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>(0, nullpt
TEST(ConstraintBlockAbs, ctr_block_abs_p_check)
{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSE", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr()))
);
@@ -60,7 +60,7 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p_check)
TEST(ConstraintBlockAbs, ctr_block_abs_p_solve)
{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSE", pose10.head<3>(), data_cov.topLeftCorner<3,3>()));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getPPtr()))
);
@@ -78,7 +78,7 @@ TEST(ConstraintBlockAbs, ctr_block_abs_p_solve)
TEST(ConstraintBlockAbs, ctr_block_abs_v_check)
{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSE", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr()))
);
@@ -87,7 +87,7 @@ TEST(ConstraintBlockAbs, ctr_block_abs_v_check)
TEST(ConstraintBlockAbs, ctr_block_abs_v_solve)
{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSE", pose10.tail<3>(), data_cov.bottomRightCorner<3,3>()));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
fea0->addConstraint(std::make_shared<ConstraintBlockAbsolute>(fea0->getFramePtr()->getVPtr()))
);
@@ -105,7 +105,7 @@ TEST(ConstraintBlockAbs, ctr_block_abs_v_solve)
TEST(ConstraintQuatAbs, ctr_block_abs_o_check)
{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSE", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()))
);
@@ -114,7 +114,7 @@ TEST(ConstraintQuatAbs, ctr_block_abs_o_check)
TEST(ConstraintQuatAbs, ctr_block_abs_o_solve)
{
- FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("FIX", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
+ FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("POSE", pose10.segment<4>(3), data_cov.block<3,3>(3,3)));
ConstraintBlockAbsolutePtr ctr0 = std::static_pointer_cast<ConstraintBlockAbsolute>(
fea0->addConstraint(std::make_shared<ConstraintQuaternionAbsolute>(fea0->getFramePtr()->getOPtr()))
);
diff --git a/src/test/gtest_constraint_imu.cpp b/src/test/gtest_constraint_imu.cpp
index cfc0ade33e9793c0bd8f48e8f080531c7657be93..efa1647b758ec88b6cff56ec62cf94b648d9cfd5 100644
--- a/src/test/gtest_constraint_imu.cpp
+++ b/src/test/gtest_constraint_imu.cpp
@@ -11,13 +11,12 @@
#include "processor_imu.h"
#include "processor_odom_3D.h"
#include "ceres_wrapper/ceres_manager.h"
-#include "constraint_fix_3D.h"
-
#include "utils_gtest.h"
#include "../src/logging.h"
#include <iostream>
#include <fstream>
+#include "../constraint_pose_3D.h"
//#define GET_RESIDUALS
@@ -2466,7 +2465,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotY, VarQ1B1B2P2Q2_InvarP1V1
featureFix_cov(5,5) = 0.1;
CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr));
FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
- ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintFix3D>(ffix->addConstraint(std::make_shared<ConstraintFix3D>(ffix)));
+ ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintPose3D>(ffix->addConstraint(std::make_shared<ConstraintPose3D>(ffix)));
//prepare problem for solving
origin_KF->getPPtr()->fix();
@@ -2524,7 +2523,7 @@ TEST_F(ConstraintIMU_ODOM_biasTest_Move_NonNullBiasRotXY, VarQ1B1B2P2Q2_InvarP1V
featureFix_cov(5,5) = 0.1;
CaptureBasePtr capfix = origin_KF->addCapture(std::make_shared<CaptureMotion>(0, nullptr, (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), 7, 6, nullptr));
FeatureBasePtr ffix = capfix->addFeature(std::make_shared<FeatureBase>("ODOM 3D", (Eigen::Vector7s() << 0,0,0, 0,0,0,1).finished(), featureFix_cov));
- ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintFix3D>(ffix->addConstraint(std::make_shared<ConstraintFix3D>(ffix)));
+ ConstraintFix3DPtr ctr_fix = std::static_pointer_cast<ConstraintPose3D>(ffix->addConstraint(std::make_shared<ConstraintPose3D>(ffix)));
//prepare problem for solving
origin_KF->getPPtr()->fix();
diff --git a/src/test/gtest_constraint_fix.cpp b/src/test/gtest_constraint_pose_2D.cpp
similarity index 84%
rename from src/test/gtest_constraint_fix.cpp
rename to src/test/gtest_constraint_pose_2D.cpp
index 7e90af10c37f0ebeb570a093a6646c2d5834dd13..220ead9cfdd33baddcc525966890ff82b0b73754 100644
--- a/src/test/gtest_constraint_fix.cpp
+++ b/src/test/gtest_constraint_pose_2D.cpp
@@ -1,14 +1,14 @@
/**
- * \file gtest_constraint_fix.cpp
+ * \file gtest_constraint_pose_2D.cpp
*
* Created on: Mar 30, 2017
* \author: jsola
*/
+#include "../constraint_pose_2D.h"
#include "utils_gtest.h"
-#include "constraint_fix.h"
#include "capture_motion.h"
#include "ceres_wrapper/ceres_manager.h"
@@ -37,11 +37,11 @@ FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeS
// Capture, feature and constraint from frm1 to frm0
CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>(0, nullptr, pose, 3, 3, nullptr));
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 2D", pose, data_cov));
-ConstraintFixPtr ctr0 = std::static_pointer_cast<ConstraintFix>(fea0->addConstraint(std::make_shared<ConstraintFix>(fea0)));
+ConstraintPose2DPtr ctr0 = std::static_pointer_cast<ConstraintPose2D>(fea0->addConstraint(std::make_shared<ConstraintPose2D>(fea0)));
////////////////////////////////////////////////////////
-TEST(ConstraintFix, check_tree)
+TEST(ConstraintPose2D, check_tree)
{
ASSERT_TRUE(problem->check(0));
}
@@ -51,7 +51,7 @@ TEST(ConstraintFix, check_tree)
// ASSERT_EIGEN_APPROX(ctr0->expectation() , delta);
//}
-TEST(ConstraintFix, solve)
+TEST(ConstraintPose2D, solve)
{
// Fix frame 0, perturb frm1
diff --git a/src/test/gtest_constraint_fix_3D.cpp b/src/test/gtest_constraint_pose_3D.cpp
similarity index 87%
rename from src/test/gtest_constraint_fix_3D.cpp
rename to src/test/gtest_constraint_pose_3D.cpp
index 76b13526c8d465f94a54efa34c099eaa25e54641..a22d361c842b21e80c2a63dcae5d04f621fc744d 100644
--- a/src/test/gtest_constraint_fix_3D.cpp
+++ b/src/test/gtest_constraint_pose_3D.cpp
@@ -6,9 +6,9 @@
*/
+#include "../constraint_pose_3D.h"
#include "utils_gtest.h"
-#include "constraint_fix_3D.h"
#include "capture_motion.h"
#include "ceres_wrapper/ceres_manager.h"
@@ -44,12 +44,12 @@ FrameBasePtr frm0 = problem->emplaceFrame(KEY_FRAME, problem->zeroState(), TimeS
// Capture, feature and constraint
CaptureBasePtr cap0 = frm0->addCapture(std::make_shared<CaptureMotion>(0, nullptr, pose7, 7, 6, nullptr));
FeatureBasePtr fea0 = cap0->addFeature(std::make_shared<FeatureBase>("ODOM 3D", pose7, data_cov));
-ConstraintFix3DPtr ctr0 = std::static_pointer_cast<ConstraintFix3D>(fea0->addConstraint(std::make_shared<ConstraintFix3D>(fea0)));
+ConstraintPose3DPtr ctr0 = std::static_pointer_cast<ConstraintPose3D>(fea0->addConstraint(std::make_shared<ConstraintPose3D>(fea0)));
////////////////////////////////////////////////////////
-TEST(ConstraintFix3D, check_tree)
+TEST(ConstraintPose3D, check_tree)
{
ASSERT_TRUE(problem->check(0));
}
@@ -59,7 +59,7 @@ TEST(ConstraintFix3D, check_tree)
// ASSERT_EIGEN_APPROX(ctr0->expectation() , delta);
//}
-TEST(ConstraintFix3D, solve)
+TEST(ConstraintPose3D, solve)
{
// Fix frame 0, perturb frm1
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;
diff --git a/src/wolf.h b/src/wolf.h
index 2978cc6032a9ae433de40ef237e94b219933b9a4..24ded0a36bf77e26ad5dd5b3e499de46495c4fbe 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -212,8 +212,8 @@ typedef enum
CTR_GPS_FIX_2D = 1, ///< 2D GPS Fix constraint.
CTR_GPS_PR_2D, ///< 2D GPS Pseudorange constraint.
CTR_GPS_PR_3D, ///< 3D GPS Pseudorange constraint.
- CTR_FIX, ///< Fix constraint (for priors).
- CTR_FIX_3D, ///< Fix constraint (for priors) in 3D.
+ CTR_POSE_2D, ///< Pose constraint (for priors) in 2D.
+ CTR_POSE_3D, ///< Pose constraint (for priors) in 3D.
CTR_FIX_BIAS, ///< Fix constraint (for priors) on bias.
CTR_ODOM_2D, ///< 2D Odometry constraint .
CTR_ODOM_3D, ///< 3D Odometry constraint .