Skip to content
Snippets Groups Projects
Commit 4e40c1ce authored by Cesar Debeunne's avatar Cesar Debeunne
Browse files

cleaning

parent 5a58b932
No related branches found
No related tags found
No related merge requests found
...@@ -162,7 +162,6 @@ int main() ...@@ -162,7 +162,6 @@ int main()
foreach(rosbag::MessageInstance const m, view) foreach(rosbag::MessageInstance const m, view)
{ {
std::cout << "--------FRAME " << frame_id << "-------------" << std::endl; std::cout << "--------FRAME " << frame_id << "-------------" << std::endl;
std::cout << problem->getLastFrame()->getP()->getState() << std::endl;
wolf_ros_objectslam::CosyObjectArray::ConstPtr cosyArray = m.instantiate<wolf_ros_objectslam::CosyObjectArray>(); wolf_ros_objectslam::CosyObjectArray::ConstPtr cosyArray = m.instantiate<wolf_ros_objectslam::CosyObjectArray>();
if (cosyArray != nullptr){ if (cosyArray != nullptr){
...@@ -179,11 +178,6 @@ int main() ...@@ -179,11 +178,6 @@ int main()
sig << 0.02, 0.02, 0.02, 0.05, 0.05, 0.05; sig << 0.02, 0.02, 0.02, 0.05, 0.05, 0.05;
cov = sig.array().matrix().asDiagonal(); cov = sig.array().matrix().asDiagonal();
ObjectDetection det = {.measurement = poseVec, .detection_score = object.score, .meas_cov = cov, .object_type = object.name}; ObjectDetection det = {.measurement = poseVec, .detection_score = object.score, .meas_cov = cov, .object_type = object.name};
std::cout << "OBJECT DETECTED" << std::endl;
std::cout << object.name << std::endl;
std::cout << poseVec << std::endl;
std::cout << object.score << std::endl;
std::cout << "------------" << std::endl;
dets.push_back(det); dets.push_back(det);
} }
TimeStamp ts(m.getTime().toSec()); TimeStamp ts(m.getTime().toSec());
......
# rosbag name # rosbag name
bag: "/demos/bag/demo_imu_long_wolf_ws.bag" bag: "/demos/bag/demo_imu_wolf.bag"
# Camera to IMU transformation # Camera to IMU transformation
unfix_extrinsic: false unfix_extrinsic: false
......
...@@ -166,6 +166,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark ...@@ -166,6 +166,7 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
unsigned int min_features_for_keyframe_; unsigned int min_features_for_keyframe_;
int nb_vote_for_every_first_; int nb_vote_for_every_first_;
bool add_3d_cstr_; bool add_3d_cstr_;
bool keyframe_voted_;
int nb_vote_; int nb_vote_;
int fps_; int fps_;
Matrix3d intrinsic_; Matrix3d intrinsic_;
......
...@@ -40,6 +40,7 @@ void ProcessorTrackerLandmarkObject::preProcess() ...@@ -40,6 +40,7 @@ void ProcessorTrackerLandmarkObject::preProcess()
{ {
//clear wolf detections so that new ones will be stored inside //clear wolf detections so that new ones will be stored inside
detections_incoming_.clear(); detections_incoming_.clear();
keyframe_voted_ = false;
auto incoming_ptr = std::dynamic_pointer_cast<CaptureObject>(incoming_ptr_); auto incoming_ptr = std::dynamic_pointer_cast<CaptureObject>(incoming_ptr_);
assert(incoming_ptr != nullptr && "Capture type mismatch. ProcessorTrackerLandmarkObject can only process captures of type CaptureObject"); assert(incoming_ptr != nullptr && "Capture type mismatch. ProcessorTrackerLandmarkObject can only process captures of type CaptureObject");
...@@ -56,31 +57,34 @@ void ProcessorTrackerLandmarkObject::postProcess() ...@@ -56,31 +57,34 @@ void ProcessorTrackerLandmarkObject::postProcess()
{ {
nb_vote_++; nb_vote_++;
// Clear the landmark map // Clear the landmark map
LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList(); if (keyframe_voted_){
for (auto iter_map = lmk_list.begin(); iter_map != lmk_list.end(); iter_map++){ // Only if a keyframe is voted so that a suppressed landmark is not assigned to
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(*iter_map); // a factor
double dt_lmk = getProblem()->getTimeStamp().get() - lmk_obj->getInitTimestamp().get(); LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList();
int number_of_factors = lmk_obj->getConstrainedByList().size(); for (auto iter_map = lmk_list.begin(); iter_map != lmk_list.end(); iter_map++){
double lmk_score = number_of_factors / dt_lmk; auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(*iter_map);
std::cout << lmk_obj->getObjectType() << std::endl; double dt_lmk = getProblem()->getTimeStamp().get() - lmk_obj->getInitTimestamp().get();
std::cout << lmk_score << std::endl; int number_of_factors = lmk_obj->getConstrainedByList().size();
double lmk_score = number_of_factors / dt_lmk;
if (number_of_factors > 10){
continue; if (number_of_factors > 10){
} lmk_score = 10;
}
if (lmk_score < lmk_score_thr_){ if (lmk_score < lmk_score_thr_){
iter_map->get()->remove(); iter_map->get()->remove();
}
} }
} }
} }
FactorBasePtr ProcessorTrackerLandmarkObject::emplaceFactor(FeatureBasePtr _feature_ptr, FactorBasePtr ProcessorTrackerLandmarkObject::emplaceFactor(FeatureBasePtr _feature_ptr,
LandmarkBasePtr _landmark_ptr) LandmarkBasePtr _landmark_ptr)
{ {
// A keyframe is voted
keyframe_voted_ = true;
return FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(_feature_ptr, return FactorBase::emplace<FactorKfLmkPose3dWithExtrinsics>(_feature_ptr,
getSensor(), getSensor(),
getLast()->getFrame(), getLast()->getFrame(),
...@@ -141,57 +145,12 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n ...@@ -141,57 +145,12 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
bool feature_already_found = false; bool feature_already_found = false;
auto feat_obj = std::static_pointer_cast<FeatureObject>(feat); auto feat_obj = std::static_pointer_cast<FeatureObject>(feat);
// If the feature was linked, there is a capture
if (feat->getCapture() != nullptr){ if (feat->getCapture() != nullptr){
feature_already_found = true; feature_already_found = true;
break; break;
} }
// Loop over the landmark to find if one is associated to feat
for(auto lmk: lmk_lst){
LandmarkObjectPtr lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
// Check if the object ID is the same
if(lmk_obj != nullptr and lmk_obj->getObjectType() == feat_obj->getObjectType()){
// Then check if the world pose is similar
// world to rob
Vector3d pos = getLast()->getFrame()->getP()->getState();
Quaterniond quat (getLast()->getFrame()->getO()->getState().data());
Eigen::Isometry3d w_M_r = Eigen::Translation<double,3>(pos.head(3)) * quat;
// rob to sensor
pos = getSensor()->getP()->getState();
quat.coeffs() = getSensor()->getO()->getState();
Eigen::Isometry3d r_M_c = Eigen::Translation<double,3>(pos.head(3)) * quat;
// camera to feat
pos = feat->getMeasurement().head(3);
quat.coeffs() = feat->getMeasurement().tail(4);
Eigen::Isometry3d c_M_f = Eigen::Translation<double,3>(pos) * quat;
// world to feat
Eigen::Isometry3d w_M_f = w_M_r * r_M_c * c_M_f;
Quaterniond quat_feat;
Eigen::Matrix3d wRf = w_M_f.linear();
quat_feat.coeffs() = R2q(wRf).coeffs().transpose();
Vector3d pos_feat = w_M_f.translation();
// world to lmk
Vector3d pos_lmk = lmk_obj->getP()->getState();
Quaterniond quat_lmk(lmk_obj->getO()->getState().data());
// Error computation
double e_pos = (pos_lmk - pos_feat).norm();
double e_rot = log_q(quat_lmk.conjugate() * quat_feat).norm();
if (e_pos < e_pos_thr_ && e_rot < e_rot_thr_){
feature_already_found = true;
break;
}
}
}
if (!feature_already_found) if (!feature_already_found)
{ {
// If the feature is not in the map & not in the list of newly detected features yet, then we add it. // If the feature is not in the map & not in the list of newly detected features yet, then we add it.
...@@ -225,9 +184,11 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const ...@@ -225,9 +184,11 @@ bool ProcessorTrackerLandmarkObject::voteForKeyFrame() const
if (too_long_since_last_KF){ if (too_long_since_last_KF){
return true; return true;
} }
// no detection in incoming capture and a minimum time since last KF has past // no detection in incoming capture and a minimum time since last KF has past
if ((detections_incoming_.size() < min_features_for_keyframe_) and enough_time_vote) if ((detections_incoming_.size() < min_features_for_keyframe_) and enough_time_vote){
return true; return true;
}
// Vote for every image processed at the beginning if possible // Vote for every image processed at the beginning if possible
if (nb_vote_ < nb_vote_for_every_first_){ if (nb_vote_ < nb_vote_for_every_first_){
...@@ -253,6 +214,7 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr ...@@ -253,6 +214,7 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
for (auto lmk : _landmarks_in) for (auto lmk : _landmarks_in)
{ {
auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk); auto lmk_obj = std::dynamic_pointer_cast<LandmarkObject>(lmk);
if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type) if(lmk_obj != nullptr and lmk_obj->getObjectType() == object_type)
{ {
......
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