Skip to content
Snippets Groups Projects
Commit c308264e authored by ydepledt's avatar ydepledt
Browse files

Debug overloaded function

parent 40bfa479
No related branches found
No related tags found
No related merge requests found
...@@ -14,6 +14,8 @@ ...@@ -14,6 +14,8 @@
namespace wolf { namespace wolf {
Isometry3d posevec_to_isometry(Vector7d pose);
WOLF_STRUCT_PTR_TYPEDEFS(ParamsCapture); WOLF_STRUCT_PTR_TYPEDEFS(ParamsCapture);
struct ParamsCapture : public ParamsBase struct ParamsCapture : public ParamsBase
......
...@@ -316,9 +316,9 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP ...@@ -316,9 +316,9 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
} }
// world to rob // world to rob
Eigen::Isometry3d w_M_r = getw_M_r(); Eigen::Isometry3d w_M_r = posevec_to_isometry(getLast()->getFrame()->getStateVector());
// rob to sensor // rob to sensor
Eigen::Isometry3d r_M_c = getr_M_c(); Eigen::Isometry3d r_M_c = posevec_to_isometry(getSensor()->getStateVector());
if (_landmarks_in.size() == 0) if (_landmarks_in.size() == 0)
return _features_out.size(); return _features_out.size();
...@@ -327,7 +327,7 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP ...@@ -327,7 +327,7 @@ unsigned int ProcessorTrackerLandmarkObject::bestLmkMatching(const LandmarkBaseP
{ {
std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType(); std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
// camera to feat // camera to feat
Eigen::Isometry3d c_M_f = getc_M_f(feat); Eigen::Isometry3d c_M_f = posevec_to_isometry(feat->getMeasurement());
if (feat->getCapture() != nullptr){ if (feat->getCapture() != nullptr){
break; break;
......
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