From 5a58b9321c226b466127c810318cc08879028c34 Mon Sep 17 00:00:00 2001
From: Cesar Debeunne <cdebeunne@nilgiri.laas.fr>
Date: Fri, 20 Aug 2021 09:42:14 +0200
Subject: [PATCH] orientation init
---
demos/cosyslam.cpp | 6 +-
demos/cosyslam_imu.cpp | 72 ++++++++++++++-----
demos/yaml/cosyslam_imu.yaml | 6 +-
demos/yaml/processor_imu.yaml | 8 +--
.../processor_tracker_landmark_object.yaml | 2 +-
.../processor_tracker_landmark_object.cpp | 54 +++-----------
6 files changed, 75 insertions(+), 73 deletions(-)
diff --git a/demos/cosyslam.cpp b/demos/cosyslam.cpp
index be0615e..9c8702b 100644
--- a/demos/cosyslam.cpp
+++ b/demos/cosyslam.cpp
@@ -82,7 +82,7 @@ int main()
rosbag::View view(bag, rosbag::TopicQuery(topics));
ros::Time bag_begin_time = view.getBeginTime();
TimeStamp init_ts(bag_begin_time.toSec());
- FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, init_ts, 0.1);
+ FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
// Variables init
Matrix6d cov;
@@ -114,8 +114,8 @@ int main()
ObjectDetection det = {.measurement = poseVec, .detection_score = object.score, .meas_cov = cov, .object_type = object.name};
dets.push_back(det);
}
- TimeStamp ts(cosyArray->header.stamp.sec, cosyArray->header.stamp.nsec);
- CaptureObjectPtr cap = std::make_shared<CaptureObject>(ts, sen, dets);
+ TimeStamp ts(m.getTime().toSec());
+ CaptureObjectPtr cap = std::make_shared<CaptureObject>(ts-init_ts, sen, dets);
cap->process();
dets.clear();
}
diff --git a/demos/cosyslam_imu.cpp b/demos/cosyslam_imu.cpp
index bf7c10e..5b2137d 100644
--- a/demos/cosyslam_imu.cpp
+++ b/demos/cosyslam_imu.cpp
@@ -61,10 +61,8 @@ int main()
bool unfix_extrinsic = config["unfix_extrinsic"].as<bool>();
std::vector<double> i_p_c_vec = config["i_p_c"].as<std::vector<double>>();
std::vector<double> i_q_c_vec = config["i_q_c"].as<std::vector<double>>();
- std::vector<double> i_q_w_vec = config["i_q_w"].as<std::vector<double>>();
Eigen::Map<Vector3d> i_p_c(i_p_c_vec.data());
Eigen::Map<Vector4d> i_q_c(i_q_c_vec.data());
- Eigen::Map<Vector4d> i_q_w(i_q_w_vec.data());
// Rosbag opening
rosbag::Bag bag;
@@ -97,38 +95,74 @@ int main()
SensorImuPtr sen_imu = std::static_pointer_cast<SensorImu>(sen_imu_base);
ProcessorBasePtr proc_imu_base = problem->installProcessor("ProcessorImu", "imu pre-integrator", "SenImu", wolf_root + "/demos/yaml/processor_imu.yaml");
ProcessorImuPtr proc_imu = std::static_pointer_cast<ProcessorImu>(proc_imu_base);
-
- // The initial orientation is given in the config file
- VectorComposite x0("POV", { Vector3d::Zero(), i_q_w, Vector3d::Zero()});
- VectorComposite sig0("POV", {Vector3d::Ones()*0.001, Vector3d::Ones()*0.1, Vector3d::Ones()*0.05});
-
- ObjectDetections dets;
-
- ///////////////
- // SLAM loop //
- ///////////////
-
+
+ // Rosbag topics target
std::vector<std::string> topics;
topics.push_back(std::string("/imu"));
topics.push_back(std::string("/cosypose"));
rosbag::View view(bag, rosbag::TopicQuery(topics));
ros::Time bag_begin_time = view.getBeginTime();
TimeStamp init_ts(bag_begin_time.toSec());
- FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
+ int frame_id = 0;
+
+ /////////////////////////////////
+ // Initial orientation with IMU//
+ /////////////////////////////////
+
+ Vector3d acc_mean;
+ int N_iter = 10;
+ foreach(rosbag::MessageInstance const m, view)
+ {
+ if (frame_id < N_iter){
+ sensor_msgs::Imu::ConstPtr cosyIMU = m.instantiate<sensor_msgs::Imu>();
+ if (cosyIMU != nullptr){
+ frame_id++;
+ Vector3d acc;
+ acc << cosyIMU->linear_acceleration.x,
+ cosyIMU->linear_acceleration.y,
+ cosyIMU->linear_acceleration.z;
+ acc_mean = acc_mean + acc;
+ }
+ }
+ }
+ acc_mean = acc_mean/N_iter;
+
+ // we use the rodriguez formula to align the acceleration to g
+ Vector3d w_g; w_g << 0,0,-9.81;
+ Vector3d w_g_norm = w_g/w_g.norm();
+ Vector3d acc_norm = acc_mean/acc_mean.norm();
+ Vector3d v = (-w_g_norm).cross(acc_norm);
+ double c = (-w_g_norm).dot(acc_norm);
+
+ Matrix3d v_skew;
+ v_skew << 0, -v(2), v(1),
+ v(2), 0, -v(0),
+ -v(1), v(0), 0;
+ Matrix3d b_R_w = Matrix3d::Identity() + v_skew + (v_skew*v_skew)/(1+c);
+ Vector4d q_init = Quaterniond(b_R_w.transpose()).coeffs();
+
+
+ ///////////////
+ // SLAM loop //
+ ///////////////
// Variables init
Matrix6d cov;
Vector6d sig;
Vector7d poseVec;
+ ObjectDetections dets;
int number_of_kf = 0;
int counter = 0;
- int frame_id = 0;
+ frame_id = 0;
+ VectorComposite x0("POV", { Vector3d::Zero(), q_init, Vector3d::Zero()});
+ VectorComposite sig0("POV", {Vector3d::Ones()*0.001, Vector3d::Ones()*0.1, Vector3d::Ones()*0.05});
+ FrameBasePtr F1 = problem->setPriorFactor(x0, sig0, 0.0, 0.1);
foreach(rosbag::MessageInstance const m, view)
{
-
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>();
if (cosyArray != nullptr){
@@ -145,6 +179,11 @@ int main()
sig << 0.02, 0.02, 0.02, 0.05, 0.05, 0.05;
cov = sig.array().matrix().asDiagonal();
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);
}
TimeStamp ts(m.getTime().toSec());
@@ -211,6 +250,7 @@ int main()
VectorComposite state_lmk;
CaptureBasePtr cap_cosy;
counter = 0;
+
for (auto& elt: problem->getTrajectory()->getFrameMap()){
TimeStamp t = elt.first;
FrameBasePtr kf = elt.second;
diff --git a/demos/yaml/cosyslam_imu.yaml b/demos/yaml/cosyslam_imu.yaml
index a636e91..063a9c0 100644
--- a/demos/yaml/cosyslam_imu.yaml
+++ b/demos/yaml/cosyslam_imu.yaml
@@ -1,9 +1,9 @@
# rosbag name
-bag: "/demos/bag/longIMU.bag"
+bag: "/demos/bag/demo_imu_long_wolf_ws.bag"
# Camera to IMU transformation
unfix_extrinsic: false
i_p_c: [-0.0111433, 0.00260534, 0.00542655]
i_q_c: [ 0.01424561, -0.00273651, 0.00528824, 0.9998808]
-# i_q_w: [ 0.81615748, 0.38330015, -0.14646648, -0.40683601] for short IMU
-i_q_w: [ 0.86842613, -0.01545895, -0.0271302, -0.49483434]
+i_q_w: [ 0.81615748, 0.38330015, -0.14646648, -0.40683601] # for short IMU
+# i_q_w: [ 0.86842613, -0.01545895, -0.0271302, -0.49483434] # for long IMU
diff --git a/demos/yaml/processor_imu.yaml b/demos/yaml/processor_imu.yaml
index b67f859..b2b6401 100644
--- a/demos/yaml/processor_imu.yaml
+++ b/demos/yaml/processor_imu.yaml
@@ -1,9 +1,9 @@
keyframe_vote:
- angle_turned: 1000
- dist_traveled: 20000.0
+ angle_turned: 45
+ dist_traveled: 0.15
max_buff_length: 100000000000
- max_time_span: 0.19999
- voting_active: true
+ max_time_span: 0.15
+ voting_active: false
name: SenImu
time_tolerance: 0.0005
type: ProcessorImu
diff --git a/demos/yaml/processor_tracker_landmark_object.yaml b/demos/yaml/processor_tracker_landmark_object.yaml
index 2a5a5b3..85e125d 100644
--- a/demos/yaml/processor_tracker_landmark_object.yaml
+++ b/demos/yaml/processor_tracker_landmark_object.yaml
@@ -15,7 +15,7 @@ intrinsic:
fx: 951.15435791
fy: 951.40795898
-lmk_score_thr: 3.0
+lmk_score_thr: 4.0
e_pos_thr: 0.1
e_rot_thr: 0.3
fps: 30
diff --git a/src/processor/processor_tracker_landmark_object.cpp b/src/processor/processor_tracker_landmark_object.cpp
index 268b567..0d0fc9b 100644
--- a/src/processor/processor_tracker_landmark_object.cpp
+++ b/src/processor/processor_tracker_landmark_object.cpp
@@ -56,47 +56,6 @@ void ProcessorTrackerLandmarkObject::postProcess()
{
nb_vote_++;
- // // Prediction with constant speed hypothesis
- // double map_size = getProblem()->getTrajectory()->getFrameMap().size();
- // auto iter = getProblem()->getTrajectory()->getFrameMap().end();
- // double dt_frame = 1.0/double(fps_);
-
- // // Check if we have enough kf
- // if (map_size > 2){
- // // Compute linear velocity
- // iter--;
- // FrameBasePtr current_pose = iter->second;
- // TimeStamp current_ts = iter->first;
- // iter--;
- // FrameBasePtr last_pose = iter->second;
- // TimeStamp last_ts = iter->first;
- // double dt_kf = current_ts.get()-last_ts.get();
- // Vector3d linear_velocity = (current_pose->getP()->getState()-last_pose->getP()->getState())/dt_kf;
-
- // if (linear_velocity(0) == 0){
- // iter--;
- // current_pose = last_pose;
- // current_ts = last_ts;
- // last_pose = iter->second;
- // last_ts = iter->first;
- // dt_kf = current_ts.get()-last_ts.get();
- // linear_velocity = (current_pose->getP()->getState()-last_pose->getP()->getState())/dt_kf;
- // }
-
- // // Compute angular velocity with log map
- // Quaterniond current_quat(current_pose->getO()->getState().data());
- // Quaterniond last_quat(last_pose->getO()->getState().data());
- // Vector3d angular_velocity = log_q(current_quat.conjugate() * last_quat)/dt_kf;
-
- // // Integrate speed on current pose
- // current_pose = getLast()->getFrame();
- // current_quat.coeffs() = current_pose->getO()->getState();
- // Vector3d new_pose = current_pose->getP()->getState() + linear_velocity*dt_frame;
- // Quaterniond new_quat = exp_q(log_q(current_quat) + angular_velocity*dt_frame);
- // getLast()->getFrame()->getP()->setState(new_pose);
- // getLast()->getFrame()->getO()->setState(new_quat.coeffs());
- // }
-
// Clear the landmark map
LandmarkBasePtrList lmk_list = getProblem()->getMap()->getLandmarkList();
for (auto iter_map = lmk_list.begin(); iter_map != lmk_list.end(); iter_map++){
@@ -104,6 +63,12 @@ void ProcessorTrackerLandmarkObject::postProcess()
double dt_lmk = getProblem()->getTimeStamp().get() - lmk_obj->getInitTimestamp().get();
int number_of_factors = lmk_obj->getConstrainedByList().size();
double lmk_score = number_of_factors / dt_lmk;
+ std::cout << lmk_obj->getObjectType() << std::endl;
+ std::cout << lmk_score << std::endl;
+
+ if (number_of_factors > 10){
+ continue;
+ }
if (lmk_score < lmk_score_thr_){
iter_map->get()->remove();
@@ -164,8 +129,6 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
const CaptureBasePtr& _capture,
FeatureBasePtrList& _features_out)
{
- std::cout << "in new feats" << std::endl;
-
// list of landmarks in the map
auto lmk_lst = getProblem()->getMap()->getLandmarkList();
for (auto feat : detections_last_)
@@ -239,7 +202,7 @@ unsigned int ProcessorTrackerLandmarkObject::detectNewFeatures(const int& _max_n
break;
}
}
-
+ std::cout << "New Features :" <<_features_out.size() <<std::endl;
return _features_out.size();
}
@@ -279,8 +242,6 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
FeatureBasePtrList& _features_out,
LandmarkMatchMap& _feature_landmark_correspondences)
{
- std::cout << "in find landmarks" << std::endl;
-
for (auto feat : detections_incoming_)
{
std::string object_type = std::static_pointer_cast<FeatureObject>(feat)->getObjectType();
@@ -336,6 +297,7 @@ unsigned int ProcessorTrackerLandmarkObject::findLandmarks(const LandmarkBasePtr
}
}
}
+ std::cout << "Features Matched :" <<_features_out.size() <<std::endl;
return _features_out.size();
}
--
GitLab