Commit 39d62a7e authored by Mederic Fourmy's avatar Mederic Fourmy
Browse files

Merge branch '19-building-a-new-visual-odometry-system' of...

Merge branch '19-building-a-new-visual-odometry-system' of https://gitlab.iri.upc.edu/mobile_robotics/wolf_projects/wolf_lib/plugins/vision into 19-building-a-new-visual-odometry-system
parents 0f60d084 dbb9ed02
......@@ -131,10 +131,10 @@ inline void FactorPixelHp::expectation(const T* const _frame_p,
using namespace Eigen;
// frames w: world; r: robot; c: camera
Matrix<T, 3, 1> p_wr(_frame_p);
Quaternion<T> q_wr(_frame_o);
Matrix<T, 3, 1> p_rc(_sensor_p);
Quaternion<T> q_rc(_sensor_o);
Map<const Matrix<T, 3, 1> > p_wr(_frame_p);
Map<const Quaternion<T> > q_wr(_frame_o);
Map<const Matrix<T, 3, 1> > p_rc(_sensor_p);
Map<const Quaternion<T> > q_rc(_sensor_o);
// camera pose in world frame: transforms from camera to world
Matrix<T, 3, 1> p_wc = p_wr + q_wr * p_rc;
......
......@@ -133,10 +133,6 @@ void ProcessorVisualOdometry::preProcess()
capture_image_incoming_->setTracksOrigin(tracks_init);
capture_image_incoming_->setTracksPrev(tracks_init);
// print a bar with the number of active tracks in incoming
std::string s(capture_image_incoming_->getKeyPoints().size(), '#');
WOLF_INFO("TRACKS: ", capture_image_incoming_->getKeyPoints().size(), " ", s);
auto __attribute__((unused)) dt_preprocess = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t1).count();
WOLF_DEBUG( "dt_preprocess (ms): " , dt_preprocess );
......@@ -310,11 +306,6 @@ void ProcessorVisualOdometry::preProcess()
capture_image_incoming_->setTracksOrigin(tracks_origin_incoming);
auto __attribute__((unused)) dt_preprocess = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t1).count();
// print a bar with the number of active tracks in incoming
std::string s(capture_image_incoming_->getKeyPoints().size(), '#');
WOLF_INFO("TRACKS: ", capture_image_incoming_->getKeyPoints().size(), " ", s);
WOLF_DEBUG( "dt_preprocess (ms): " , dt_preprocess );
return;
......@@ -480,16 +471,16 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmark(FeatureBasePtr _feat)
Eigen::Vector3d point3d;
point3d = pinhole::backprojectPoint(
getSensor()->getIntrinsic()->getState(),
(std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),
point2d);
getSensor()->getIntrinsic()->getState(),
(std::static_pointer_cast<SensorCamera>(getSensor()))->getCorrectionVector(),
point2d);
//double distance = params_bundle_adjustment_->distance; // arbitrary value
// double distance = params_bundle_adjustment_->distance; // arbitrary value
double distance = 1;
Eigen::Vector4d vec_homogeneous_c;
vec_homogeneous_c = {point3d(0),point3d(1),point3d(2),point3d.norm()/distance};
//TODO: lmk from camera to world coordinate frame.
// lmk from camera to world coordinate frame.
Transform<double,3,Isometry> T_w_r
= Translation<double,3>(feat_pi->getFrame()->getP()->getState())
* Quaterniond(_feat->getFrame()->getO()->getState().data());
......@@ -533,6 +524,16 @@ void ProcessorVisualOdometry::postProcess()
++track_it;
}
// print a bar with the number of active tracks in incoming
unsigned int n = capture_image_incoming_->getKeyPoints().size();
std::string s(n/2, '#');
WOLF_INFO("TRACKS: ", n, " ", s);
n = getProblem()->getMap()->getLandmarkList().size();
s = std::string(n/2, '-');
WOLF_INFO("LMARKS: ", n, " ", s);
}
bool ProcessorVisualOdometry::voteForKeyFrame() const
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment