contrib/brl/bmvl/brct/brct_epi_reconstructor.h

Go to the documentation of this file.

#ifndef brct_epi_reconstructor_h_
#define brct_epi_reconstructor_h_

#if defined(_MSC_VER) && ( _MSC_VER > 1000 )
#pragma once
#endif // _MSC_VER > 1000
//:
// \file
// \brief A class to attack structure from motion problem
//
//  Input:
//  -  A set of tracked vdgl_digital_curve(s). Currently these
//     curves are read in from a track file, which is generated by
//     a 2-d tracking algorithm. The tracks are stored in tracks_.
//  -  A timestamp file that gives the observation time for each frame
//     in milliseconds.
//  -  The calibration matrix K_ for the camera.  Currently this
//     matrix is hard-coded in ::init_cam_intrinsic()
//  -  Initial epipole image location e_, a 2-d point.
//
//  Internals:
//   The camera translation is computed from the matched curves and an
//   assumed epipolar geometry.  This class is a heavily modified version
//   of the original Kalman filter class written by Kongbin Kang.
//
// \author J.L. Mundy
// \verbatim
//  Modifications
//   Initial version Dec. 2003
//   The class doe
// \endverbatim
//
//////////////////////////////////////////////////////////////////////

#include <vcl_vector.h>
#include <vnl/vnl_double_2.h>
#include <vnl/vnl_double_3.h>
#include <vnl/vnl_double_3x3.h>
#include <vnl/vnl_double_3x4.h>
#include <vnl/vnl_vector_fixed.h>
#include <vnl/vnl_matrix_fixed.h>
#include <vnl/vnl_matrix.h>
#include <vdgl/vdgl_digital_curve_sptr.h>
#include <bugl/bugl_gaussian_point_2d.h>
#include <vgl/vgl_point_2d.h>
#include <vgl/vgl_point_3d.h>
#include <bugl/bugl_curve_3d.h>

class brct_epi_reconstructor
{
 public:

  //: write result of a frame into a file
  void write_results(const char* fname);
  vgl_point_2d<double> get_cur_epipole() const;
  //: get backprojection for debugging
  vcl_vector<vnl_matrix<double> > get_back_projection() const;

  //: predict next curve.
  vnl_matrix<double> get_predicted_curve();
  vnl_double_3 get_next_motion(vnl_double_3 v);
  vcl_vector<vgl_point_2d<double> > get_pre_observes();
  vcl_vector<vgl_point_2d<double> > get_cur_observes();
  vcl_vector<vgl_point_2d<double> > get_next_observes();
  //joe version
  vcl_vector<vgl_point_2d<double> > get_joe_pre_observes();
  vcl_vector<vgl_point_2d<double> > get_joe_cur_observes();
  vcl_vector<vgl_point_2d<double> > get_joe_next_observes();


  vcl_vector<vgl_point_3d<double> > get_local_pts(); // will be superseded
  bugl_curve_3d get_curve_3d();

  //: read all the data including time stamps and  tracks.
  void read_data(const char* fname);

  //: read vishual tracker result out of a file
  vcl_vector<vdgl_digital_curve_sptr> read_track_file(char* fname);


  //: initialize the kalman filter states
  void init();
  void init_epipole(double x, double y);
  void inc();
  vnl_double_2 projection(const vnl_double_3x4 &P, const vnl_double_3 &X);

  //: constructors
  //
  brct_epi_reconstructor();
  brct_epi_reconstructor(const char* fname);
  virtual ~brct_epi_reconstructor();

  //: direct access
  void add_track(vcl_vector<vdgl_digital_curve_sptr> const& track);

  //: print track data
  void print_track(const int track_index, const int frame);

  //: print motion grouping histogram
  void print_motion_array();
 protected:
  //: read time stamp
  vcl_vector<double> read_timestamp_file(char* fname);

  //: if the zero probability returned, the matched point is a outlier
  double matched_point_prob(vnl_double_2& z, vnl_double_2& z_pred);

  //: get time interval from ith frame to j-th frame
  vnl_matrix_fixed<double, 6, 6> get_transit_matrix(int i, int j);

  //: update the confidence for each 3d point
  void update_confidence();

  //: update the matched points in the next frame using closest neighbour.
  void update_observes(const vnl_double_3x4 &P, int iframe);
  void update_observes_joe(int iframe);
  void init_velocity();

  //: set linearized observation matrix
  vnl_matrix_fixed<double, 2, 6> get_H_matrix(vnl_double_3x4 &P, vnl_double_3 &Y);

  //: compute projective matrix from predicted position
  vnl_double_3x4 get_projective_matrix(const vnl_double_3 &v) const;

  void init_covariant_matrix();
  void init_cam_intrinsic();
  void init_state_3d_estimation();
  void init_transit_matrix();

  //: utility functions
  bool match_point(vdgl_digital_curve_sptr const& dc,
                   bugl_gaussian_point_2d<double>& p0,
                   double grad_angle,
                   bugl_gaussian_point_2d<double>& p);

  vcl_vector<bugl_gaussian_point_2d<double> >
    get_cur_joe_observes(int frame);
 private:
   //: position and confidence of feature samples
  bugl_curve_3d curve_3d_;

  vcl_vector<double> prob_;

  //: the set of 2-d points in each frame, used for matching
  vcl_vector<vcl_vector<bugl_gaussian_point_2d<double> > > observes_;

  //: the set of 2-d points in each frame grouped by tracked curves
  vcl_vector<vcl_vector<vcl_vector<bugl_gaussian_point_2d<double> > > > joe_observes_;

  //: the gradient angles corresponding to frame 0 matched tracked curves
  vcl_vector<vcl_vector<double> > grad_angles_;

  //: each element represents image capture time for each frame.
  vcl_vector<double> time_tick_;

  //: each element of the vector represents a projection of the same 3D curves.
  vcl_vector<vcl_vector<vdgl_digital_curve_sptr> > tracks_;

  //: the sequence of translations
  vcl_vector<vnl_double_3> motions_;

  //: current frame position in history pool
  int cur_pos_;
  int queue_size_;
  int num_points_;

  //: how much the queue has been used
  int memory_size_;

  //: state vector
  vnl_vector_fixed<double, 6> X_;

  //: covariant matrix of state vector
  vnl_matrix_fixed<double, 6, 6> Q_;

  //: constraint kalman gain matrix
  vnl_matrix_fixed<double, 6, 2> G_;

  //: initial covariant matrix of state vector
  vnl_matrix_fixed<double, 2, 2> R_;

  //: covariant matrix of 2D projection
  vnl_matrix_fixed<double, 6, 6> Q0_;

  //: initial epipole
  vnl_double_2* e_;

  //: camera intrinsic parameters
  vnl_double_3x3 K_;

  //: used to denote outlier point in image
  static const double large_num_;
  //: debug flag
  bool debug_;
};

#endif // brct_epi_reconstructor_h_

---