laserNAVCalib.h

Go to the documentation of this file.

#ifndef _LASER_NAV_CALIB_H
#define _LASER_NAV_CALIB_H

#include <vector>

// convert rad 2 deg
#include "mathConstants.h"

#include <pcl/common/common_headers.h>

typedef pcl::PointCloud<pcl::PointNormal> pointCloud;

typedef std::pair<pointCloud::Ptr, pointCloud::Ptr> last_and_latest;

namespace filter
{
class LaserNAVCalibration
{
public:
  enum e_registrationMethod {
    E_STANDARD
    , E_LEVENBERG_MARQUARDT
  };
private:
  static const unsigned int nr_unknowns = 6;
  static unsigned int scans_between_last_and_latest;
  double sampled_scans;
  unsigned int scan_counter;
  bool plot_all_movements;
  bool calibration_wanted;

  double mounting_pose[nr_unknowns];
  double pos_x, pos_y, pos_z, phi, theta, psi;

  pointCloud::Ptr cloud_to_draw;
  pointCloud::Ptr last;
  pointCloud::Ptr latest;
  static pointCloud::Ptr last_to_draw;
  static pointCloud::Ptr latest_to_draw;
  std::vector< last_and_latest > data_set;
  unsigned int nr_scan_pairs;
  static size_t worst_pair;
  static double worst_score;
  static double avr_cal_err;
  static e_registrationMethod ICPMethod;
  static bool downsampling;
  static bool bilateral_filtering;
  static bool calculate_normals;
  static bool covariance_sampling;
  static bool initial_aligning;
  bool add_to_data_set;
  bool init;
  std::stringstream filename;

  void print(
    double pos_x, double pos_y, double pos_z
    , double phi, double theta, double psi
    , std::string name = "Transformation");

  void print(
    Eigen::Affine3d* transformation_to_print
    , std::string name = "Transformation");

  void print(
    Eigen::Matrix4d* transformation_to_print
    , std::string name = "Transformation");

  static bool exists(const std::string& name);

  static void plot(
    std::string header
    , std::string data
    , std::string filename);

  static Eigen::Matrix4d getScanTransformationFrom(
    pointCloud::Ptr last
    , pointCloud::Ptr latest
    , double* icp_fitness_score
    , double iterations);

  static Eigen::Matrix4d getMovementFrom(
    pointCloud::Ptr last
    , pointCloud::Ptr latest);

  static double evaluateICPFitnessScore(
    pointCloud::Ptr first
    , pointCloud::Ptr second
    , double pos_x
    , double pos_y
    , double pos_z
    , double phi
    , double theta
    , double psi
  );

  static void getMountingPose(
    const double *par
    , int m_dat
    , const void *data
    , double *fvec
    , int *info );

public:
  LaserNAVCalibration(
    double pos_x = 0.0, double pos_y = 0.0, double pos_z = 0.0
        , double phi = math::c_DEG_TO_RAD * 0.0
                       , double theta = math::c_DEG_TO_RAD * 0.0
                                        , double psi = math::c_DEG_TO_RAD * 0.0);

  virtual ~LaserNAVCalibration();

  const pointCloud::Ptr getLatestCloud(void)const;

  const pointCloud::Ptr getLastCloud(void)const;

  void setDownsampling(bool new_state);
  bool getDownsampling(void);

  void setBilateralFiltering(bool new_state);
  bool getBilateralFiltering(void);

  void setCalculateNormals(bool new_state);
  bool getCalculateNormals(void);

  void setCovarianceSampling(bool new_state);
  bool getCovarianceSampling(void);

  void setInitialAligning(bool new_state);
  bool getInitialAligning(void);

  void addToDataSet(void);

  void resetDataSet(void);

  void optimizeDataSet(void);

  void setScansBetweenLastAndLatest(unsigned int scans_between_last_and_latest);

  virtual void update(pointCloud::Ptr cloud_to_draw);
};
}  // filter
#endif  // _LASER_NAV_CALIB_H