void
fundamentalFromCorrespondences8PointRobust( const Mat_<double> &_x1,
const Mat_<double> &_x2,
double max_error,
Matx33d &_F,
vector<int> &_inliers,
double outliers_probability )
{
libmv::Mat x1, x2;
libmv::Mat3 F;
libmv::vector<int> inliers;
cv2eigen( _x1, x1 );
cv2eigen( _x2, x2 );
libmv::FundamentalFromCorrespondences8PointRobust( x1, x2, max_error, &F, &inliers, outliers_probability );
eigen2cv( F, _F );
// transform from libmv::vector to std::vector
int n = inliers.size();
_inliers.resize(n);
for( int i=0; i < n; ++i )
{
_inliers[i] = inliers.at(i);
}
}
void
applyTransformationToPoints( const Mat_<T> &_points,
const Mat_<T> &_T,
Mat_<T> _transformed_points )
{
libmv::Mat points, transformed_points;
libmv::Mat3 Tr;
cv2eigen( _points, points );
cv2eigen( _T, Tr );
libmv::ApplyTransformationToPoints( points, Tr, &transformed_points );
eigen2cv( transformed_points, _transformed_points );
}
void
calibrate_stereo(const observation_pts_v_t& left_points, const observation_pts_v_t& right_points,
const object_pts_v_t& object_points, const cv::Size& image_size, PinholeCameraModel& left,
PinholeCameraModel& right)
{
PinholeCameraModel camera;
StereoCameraModel scam;
std::vector<cv::Mat> rvecs, tvecs;
cv::Mat K_left, K_right, D_right, D_left, R, T, E, F;
int flags =cv::CALIB_FIX_PRINCIPAL_POINT| cv::CALIB_FIX_ASPECT_RATIO /*| cv::CALIB_ZERO_TANGENT_DIST|cv::CALIB_FIX_PRINCIPAL_POINT| cv::CALIB_FIX_K1 | cv::CALIB_FIX_K2 |cv::CALIB_FIX_K3*/;
cv::Size left_size(image_size), right_size(image_size);
double left_rms = cv::calibrateCamera(object_points, left_points, left_size, K_left, D_left, rvecs, tvecs, flags);
double right_rms = cv::calibrateCamera(object_points, right_points, right_size, K_right, D_right, rvecs, tvecs,
flags);
double stereo_rms = cv::stereoCalibrate(object_points, left_points, right_points, K_left, D_left, K_right, D_right,
left_size, R, T, E, F,
cv::TermCriteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 500, 1e-6),
cv::CALIB_FIX_INTRINSIC | flags);
std::cout << "image size:" << image_size.width << ":" << image_size.height << std::endl;
std::cout << "R:" << R << std::endl;
std::cout << "T:" << T << std::endl;
std::cout << "D:" << D_left << std::endl;
std::cout << "left rms:" << left_rms << " right rms:" << right_rms << " stereo rms:" << stereo_rms << std::endl;
left.setParams(left_size, K_left, D_left, cv::Mat_<double>::eye(3, 3), K_left);
right.setParams(right_size, K_right, D_right, cv::Mat_<double>::eye(3, 3), K_right);
left.writeCalibration("left.yml");
right.writeCalibration("right.yml");
Eigen::Matrix3d Re;
Eigen::Vector3d Te;
Eigen::Matrix4d Pe = Eigen::Matrix4d::Identity();
cv2eigen(R, Re);
cv2eigen(T.t(), Te);
std::cout << Te << std::endl;
Pe.block(0, 0, 3, 3) = Re;
Pe.block(0, 3, 3, 1) = Te;
Pose P;
P.transform.matrix() = Pe;
scam.setParams(left, right, P);
scam.writeCalibration("stereo.yml");
}
// Split the raw image into channels and store in Eigen Matrices.
std::vector<Eigen::MatrixXf> imread(const string& path) {
cv::Mat mat = cv::imread(path);
std::vector<cv::Mat> rgb;
cv::split(mat, rgb);
std::vector<Eigen::MatrixXf> eigen(rgb.size());
for (int i = 0; i < mat.channels(); ++i) {
cv2eigen(rgb[i], eigen[i]);
}
return eigen;
}
// update the camera state given a new camera pose
void MotionModel::UpdateCameraPose(const cv::Point3d& newPosition, const cv::Vec4d& newOrientation)
{
// Compute linear velocity
cv::Vec3d newLinearVelocity( newPosition - position_ );
// In order to be robust against fast camera movements linear velocity is smoothed over time
newLinearVelocity = (newLinearVelocity + linearVelocity_) * 0.5;
// compute rotation between q1 and q2: q2 * qInverse(q1);
Eigen::Quaterniond newAngularVelocity = cv2eigen( newOrientation ) * orientation_.inverse();
// In order to be robust against fast camera movements angular velocity is smoothed over time
newAngularVelocity = newAngularVelocity.slerp(0.5, angularVelocity_);
newAngularVelocity.normalize();
// Update the current state variables
position_ = newPosition;
orientation_ = cv2eigen( newOrientation );
linearVelocity_ = newLinearVelocity;
angularVelocity_ = newAngularVelocity;
}
void
isotropicPreconditionerFromPoints( const Mat_<T> &_points,
Mat_<T> _T )
{
libmv::Mat points;
libmv::Mat3 Tr;
cv2eigen( _points, points );
libmv::IsotropicPreconditionerFromPoints( points, &Tr );
eigen2cv( Tr, _T );
}
// RQ decomposition HZ A4.1.1 pag.579
void
KRt_From_P( const Matx34d &_P, Matx33d &_K, Matx33d &_R, Vec3d &_t )
{
libmv::Mat34 P;
libmv::Mat3 K, R;
libmv::Vec3 t;
cv2eigen( _P, P );
libmv::KRt_From_P( P, &K, &R, &t );
eigen2cv( K, _K );
eigen2cv( R, _R );
eigen2cv( t, _t );
}
void Eigentransformation::createEigenSpace(vector<Mat> &trainingSet, Mat &p, Mat &avg, Mat &vecs, Mat &valsDiag, Mat &eigenSpace){
Size imSize = trainingSet[0].size();
avg = Mat::zeros(imSize, CV_32F);
Mat temp = Mat::zeros(imSize, CV_32F);
int n = trainingSet.size();
for(int i=0; i<n ; i++){
trainingSet[i].convertTo(temp, CV_32F);
avg += temp;
}
avg = avg/n;
p = Mat::zeros(imSize.area(), n, CV_32F);
for(int i=0; i<n; i++){
trainingSet[i].convertTo(temp, CV_32F);
p.col(i) = temp.reshape(1, imSize.area()) - avg.reshape(1, imSize.area());
}
Mat ptp = (p.t())*p;
Mat vals;
MatrixXf e_M;
cv2eigen(ptp, e_M);
EigenSolver<MatrixXf> es(e_M);
eigen2cv(MatrixXf(es.eigenvectors().real()), vecs);
eigen2cv(MatrixXf(es.eigenvalues().real()), vals);
valsDiag = Mat::zeros(vecs.size(), CV_32F);
float aux;
for(int i=0; i<n; i++){
aux = 1/sqrt(vals.at<float>(i));
if(aux!=aux)
valsDiag.at<float>(i,i) = 0;
else
valsDiag.at<float>(i,i) = aux;
}
eigenSpace = p*vecs*valsDiag;
}
Isometry3d cvmat2eigen(Mat &r, Mat &t)
{
//rotation vector to matrix
Mat R;
Rodrigues(r, R);
//opencv matrix to eigen matrix format
Matrix3d re;
cv2eigen(R, re);
AngleAxisd angle(re);
//3x3 identity matrix
Isometry3d T = Isometry3d::Identity();
T = angle;
T(0,3) = t.at<double>(0,0);
T(1,3) = t.at<double>(0,1);
T(2,3) = t.at<double>(0,2);
return T;
}
MotionModel::MotionModel(const cv::Point3d& initialPosition, const cv::Vec4d& initialOrientation)
: position_( initialPosition ), linearVelocity_(0, 0, 0)
{
orientation_ = cv2eigen( initialOrientation );
angularVelocity_ = cv2eigen( cv::Vec4d(1, 0, 0, 0) );
}
void StereoFrame::CreatePoints(
const RowMatcher& matcher,
const std::vector<cv::KeyPoint>& keypointsLeft, const cv::Mat& allDescriptorsLeft,
const std::vector<cv::KeyPoint>& keypointsRight, const cv::Mat& allDescriptorsRight,
std::aligned_vector<MapPoint>& points, std::list<std::pair<size_t, size_t>>& matches
)
{
#if defined(SHOW_PROFILING) && PROFILE_INTERNAL_CREATE
sptam::Timer t_match, t_triangulate;
t_match.start();
#endif
// per-row matcher for stereo rectify images
std::vector<cv::DMatch> cvMatches;
matcher.match(keypointsLeft, allDescriptorsLeft, keypointsRight, allDescriptorsRight, cvMatches);
#if defined(SHOW_PROFILING) && PROFILE_INTERNAL_CREATE
t_match.stop();
#endif
// Return if no matches were found
if ( cvMatches.empty() ) {
std::cerr << "No matches found for triangulation" << std::endl;
return;
}
const Camera& cameraLeft = frameLeft_.GetCamera();
const Camera& cameraRight = frameRight_.GetCamera();
// Compute Projections matrices
cv::Matx34d projectionLeft = eigen2cv( cameraLeft.GetProjection() );
cv::Matx34d projectionRight = eigen2cv( cameraRight.GetProjection() );
const std::vector<cv::DMatch>& goodMatches = cvMatches;
//std::cout << "F: Correct (by F) / Total matches: " << goodMatches.size() << " / " << cvMatches.size() << std::endl;
std::vector<cv::Point2d> matchedPointsLeft;
std::vector<cv::Point2d> matchedPointsRight;
matchedPointsLeft.reserve( goodMatches.size() );
matchedPointsRight.reserve( goodMatches.size() );
// initialize the points
for ( auto match : goodMatches ) {
matchedPointsLeft.push_back( keypointsLeft[ match.queryIdx ].pt );
matchedPointsRight.push_back( keypointsRight[ match.trainIdx ].pt );
}
#if defined(SHOW_PROFILING) && PROFILE_INTERNAL_CREATE
t_triangulate.start();
#endif
// Triangulate 3D points from both cameras
cv::Mat point3DHomos;
cv::triangulatePoints(
projectionLeft, projectionRight, matchedPointsLeft, matchedPointsRight, point3DHomos
);
#if defined(SHOW_PROFILING) && PROFILE_INTERNAL_CREATE
t_triangulate.stop();
#endif
// Filter some more by viewing frustum and fill the return parameters
for(int i = 0; i < point3DHomos.cols; ++i)
{
Eigen::Vector3d point = cv2eigen( toInHomo(cv::Vec4d( point3DHomos.col(i) )) );
// if the point is not in range of any camera it is discarded
if( not cameraLeft.CanView( point ) || not cameraRight.CanView( point ) )
continue;
// this indexes are for the original unmatched collections
size_t idxLeft = goodMatches[i].queryIdx;
size_t idxRight = goodMatches[i].trainIdx;
Eigen::Vector3d normal = point - frameLeft_.GetPosition();
normal.normalize();
points.push_back( MapPoint( point, normal, allDescriptorsLeft.row( idxLeft ), INITIAL_POINT_COVARIANCE ) );
matches.push_back( std::pair<size_t, size_t>(idxLeft, idxRight) );
}
#if defined(SHOW_PROFILING) && PROFILE_INTERNAL_CREATE
WriteToLog(" xx triangulate_points-create-match ", t_match);
WriteToLog(" xx triangulate_points-create-triangulate ", t_triangulate);
#endif
}
