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

orientation init

parent b776730e
No related branches found
No related tags found
No related merge requests found
......@@ -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();
}
......
......@@ -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;
......
# 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
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
......
......@@ -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
......
......@@ -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();
}
......
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