Skip to content
Snippets Groups Projects
Commit 7ac28eff authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Rename CaptureFix -> CapturePose

parent 99dba2a4
No related branches found
No related tags found
1 merge request!150Rename XxxxFix(plain, 2D or 3D) to XxxxPose(plain, 2D or 3D)
......@@ -3,7 +3,7 @@
namespace wolf{
CapturePose::CapturePose(const TimeStamp& _ts, SensorBasePtr _sensor_ptr, const Eigen::VectorXs& _data, const Eigen::MatrixXs& _data_covariance) :
CaptureBase("FIX", _ts, _sensor_ptr),
CaptureBase("POSE", _ts, _sensor_ptr),
data_(_data),
data_covariance_(_data_covariance)
{
......@@ -18,16 +18,16 @@ CapturePose::~CapturePose()
void CapturePose::emplaceFeatureAndConstraint()
{
// Emplace feature
FeatureFixPtr feature_fix = std::make_shared<FeatureFix>(data_,data_covariance_);
addFeature(feature_fix);
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_fix->addConstraint(std::make_shared<ConstraintPose2D>(feature_fix));
feature_pose->addConstraint(std::make_shared<ConstraintPose2D>(feature_pose));
else if (data_.size() == 7 && data_covariance_.rows() == 6 && data_covariance_.cols() == 6 )
feature_fix->addConstraint(std::make_shared<ConstraintPose3D>(feature_fix));
feature_pose->addConstraint(std::make_shared<ConstraintPose3D>(feature_pose));
else
throw std::runtime_error("Wrong data size in CaptureFix. Use 3 for 2D. Use 7 for 3D.");
throw std::runtime_error("Wrong data size in CapturePose. Use 3 for 2D. Use 7 for 3D.");
}
......
......@@ -3,9 +3,9 @@
//Wolf includes
#include "capture_base.h"
#include "feature_fix.h"
#include "constraint_pose_2D.h"
#include "constraint_pose_3D.h"
#include "feature_pose.h"
//std includes
//
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment