99 lines
2.4 KiB
C++
99 lines
2.4 KiB
C++
/*
|
|
* COPYRIGHT AND PERMISSION NOTICE
|
|
* Penn Software MSCKF_VIO
|
|
* Copyright (C) 2017 The Trustees of the University of Pennsylvania
|
|
* All rights reserved.
|
|
*/
|
|
|
|
#ifndef MSCKF_VIO_CAM_STATE_H
|
|
#define MSCKF_VIO_CAM_STATE_H
|
|
|
|
#include <map>
|
|
#include <vector>
|
|
#include <Eigen/Dense>
|
|
|
|
#include "imu_state.h"
|
|
|
|
namespace msckf_vio {
|
|
|
|
struct Frame{
|
|
cv::Mat image;
|
|
double exposureTime_ms;
|
|
};
|
|
|
|
typedef std::map<StateIDType, Frame, std::less<StateIDType>,
|
|
Eigen::aligned_allocator<
|
|
std::pair<StateIDType, Frame> > > movingWindow;
|
|
|
|
struct IlluminationParameter{
|
|
double frame_bias;
|
|
double frame_gain;
|
|
double feature_bias;
|
|
double feature_gain;
|
|
};
|
|
|
|
struct CameraCalibration{
|
|
std::string distortion_model;
|
|
cv::Vec2i resolution;
|
|
cv::Vec4d intrinsics;
|
|
cv::Vec4d distortion_coeffs;
|
|
movingWindow moving_window;
|
|
cv::Mat featureVisu;
|
|
};
|
|
|
|
|
|
|
|
/*
|
|
* @brief CAMState Stored camera state in order to
|
|
* form measurement model.
|
|
*/
|
|
struct CAMState {
|
|
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
|
|
|
|
// An unique identifier for the CAM state.
|
|
StateIDType id;
|
|
|
|
// Time when the state is recorded
|
|
double time;
|
|
|
|
// Orientation
|
|
// Take a vector from the world frame to the camera frame.
|
|
Eigen::Vector4d orientation;
|
|
|
|
// Position of the camera frame in the world frame.
|
|
Eigen::Vector3d position;
|
|
|
|
// Illumination Information of the frame
|
|
IlluminationParameter illumination;
|
|
|
|
// These two variables should have the same physical
|
|
// interpretation with `orientation` and `position`.
|
|
// There two variables are used to modify the measurement
|
|
// Jacobian matrices to make the observability matrix
|
|
// have proper null space.
|
|
Eigen::Vector4d orientation_null;
|
|
Eigen::Vector3d position_null;
|
|
|
|
// Takes a vector from the cam0 frame to the cam1 frame.
|
|
static Eigen::Isometry3d T_cam0_cam1;
|
|
|
|
CAMState(): id(0), time(0),
|
|
orientation(Eigen::Vector4d(0, 0, 0, 1)),
|
|
position(Eigen::Vector3d::Zero()),
|
|
orientation_null(Eigen::Vector4d(0, 0, 0, 1)),
|
|
position_null(Eigen::Vector3d(0, 0, 0)) {}
|
|
|
|
CAMState(const StateIDType& new_id ): id(new_id), time(0),
|
|
orientation(Eigen::Vector4d(0, 0, 0, 1)),
|
|
position(Eigen::Vector3d::Zero()),
|
|
orientation_null(Eigen::Vector4d(0, 0, 0, 1)),
|
|
position_null(Eigen::Vector3d::Zero()) {}
|
|
};
|
|
|
|
typedef std::map<StateIDType, CAMState, std::less<int>,
|
|
Eigen::aligned_allocator<
|
|
std::pair<const StateIDType, CAMState> > > CamStateServer;
|
|
} // namespace msckf_vio
|
|
|
|
#endif // MSCKF_VIO_CAM_STATE_H
|