├── .gitignore ├── README.md ├── Calibration ├── Makefile └── calibration.cpp └── MonoSLAM ├── Makefile ├── quaternion.hpp ├── feature.cpp ├── feature.hpp ├── camera.hpp ├── quaternion.cpp ├── camera.cpp ├── main.cpp ├── util.hpp ├── map.hpp ├── util.cpp └── map.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | *.o 2 | *.xml 3 | main -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # MonoSLAM 2 | A concise and thoroughly documented C++ implementation of the seminal work by [Davison et *al.* 2007](https://www.doc.ic.ac.uk/~ajd/Publications/davison_etal_pami2007.pdf), with parallel feature integration. 3 | 4 | Runs at 30 Hz on commodity hardware, check the [sample videos](https://www.youtube.com/watch?v=zg3O0ZN1_rI&list=PLQjxkIPDI8hgJj4gUFwuYeaorkd8JWDpo). 5 | -------------------------------------------------------------------------------- /Calibration/Makefile: -------------------------------------------------------------------------------- 1 | # Compiler 2 | CC = g++ 3 | 4 | # Source files 5 | FILES_C = calibration.cpp 6 | 7 | # Executable 8 | TARGET = main 9 | 10 | # Compilation options 11 | CFLAGS = -Wall -g -std=c++14 -march=native -O3 -funroll-loops `pkg-config --cflags opencv` 12 | 13 | # Linker options 14 | LFLAGS = `pkg-config --libs opencv` 15 | 16 | FILES_O = $(subst .cpp,.o,$(FILES_C)) 17 | 18 | $(TARGET): $(FILES_O) Makefile 19 | $(CC) $(FILES_O) -o $(TARGET) $(LFLAGS) 20 | 21 | %.o: %.cpp Makefile 22 | $(CC) $(CFLAGS) -c $< 23 | 24 | all: $(TARGET) 25 | 26 | clean: 27 | rm $(FILES_O) $(TARGET) -------------------------------------------------------------------------------- /MonoSLAM/Makefile: -------------------------------------------------------------------------------- 1 | # Compiler 2 | CC = g++ 3 | 4 | # Source files 5 | FILES_C = camera.cpp feature.cpp main.cpp map.cpp quaternion.cpp util.cpp 6 | 7 | # Executable 8 | TARGET = main 9 | 10 | # Compilation options 11 | CFLAGS = -Wall -g -std=c++14 -march=native -O3 -funroll-loops `pkg-config --cflags opencv` 12 | 13 | # Linker options 14 | LFLAGS = `pkg-config --libs opencv` 15 | 16 | FILES_O = $(subst .cpp,.o,$(FILES_C)) 17 | 18 | $(TARGET): $(FILES_O) Makefile 19 | $(CC) $(FILES_O) -o $(TARGET) $(LFLAGS) 20 | 21 | %.o: %.cpp Makefile 22 | $(CC) $(CFLAGS) -c $< 23 | 24 | all: $(TARGET) 25 | 26 | clean: 27 | rm $(FILES_O) $(TARGET) -------------------------------------------------------------------------------- /MonoSLAM/quaternion.hpp: -------------------------------------------------------------------------------- 1 | #ifndef QUATERNION_H 2 | #define QUATERNION_H 3 | 4 | #include "opencv2/core/core.hpp" 5 | 6 | /* 7 | * Builds a rotation quaternion from a (non-zero) rotation vector. 8 | */ 9 | cv::Mat computeQuaternion(const cv::Mat&); 10 | 11 | /* 12 | * Computes the Hamilton product of two quaternions. 13 | */ 14 | cv::Mat quaternionMultiply(const cv::Mat&, const cv::Mat&); 15 | 16 | /* 17 | * Computes a rotation matrix from a rotation quaternion. In particular, if the 18 | * quaternion is an orientation quaternion, which rotates a reference frame A to 19 | * its final pose B, the matrix is a change of basis from B to A. 20 | */ 21 | cv::Mat getRotationMatrix(const cv::Mat&); 22 | 23 | /* 24 | * Computes the inverse quaternion. 25 | */ 26 | cv::Mat quaternionInv(const cv::Mat&); 27 | 28 | /* 29 | * Computes the matrices of the derivatives of R(q)^t with respect to the quaternion 30 | * components. That is, the entries of Ri are the derivatives of the entries of the 31 | * transpose of the matrix returned by getRotationMatrix(q) with respect to the ith 32 | * quaternion component. 33 | */ 34 | void getRotationMatrixDerivatives(const cv::Mat& q, cv::Mat& R1, cv::Mat& R2, cv::Mat& R3, cv::Mat& R4); 35 | 36 | #endif -------------------------------------------------------------------------------- /MonoSLAM/feature.cpp: -------------------------------------------------------------------------------- 1 | #include "feature.hpp" 2 | 3 | using namespace cv; 4 | 5 | /* 6 | * Constructor for a fully initialized feature. It is a convenient method used only 7 | * during the map initialization step, when manually initialized features are added 8 | * with zero position uncertainty. 9 | * 10 | * image_ Camera frame at the time the feature is initialized 11 | * roi_ Feature patch 12 | * normal_ Patch normal vector 13 | * R_ Camera rotation (world to camera) at feature initialization 14 | * t_ Camera position, in world coordinates, at feature initialization 15 | */ 16 | Feature::Feature(const Mat& image_, const Rect& roi_, const Mat& normal_, 17 | const Mat& R_, const Mat& t_) { 18 | 19 | image_.copyTo(image); 20 | roi = roi_; 21 | normal_.copyTo(normal); 22 | R_.copyTo(R); 23 | t_.copyTo(t); 24 | 25 | matchingFails = 0; 26 | matchingAttempts = 1; 27 | } 28 | 29 | /* 30 | * Constructor for a pre-initialized (candidate) feature. 31 | * 32 | * image_ Camera frame at the time the feature is initialized 33 | * roi_ Feature patch 34 | * R_ Camera rotation (world to camera) at feature initialization 35 | * t_ Camera position, in world coordinates, at feature initialization 36 | * dir_ Feature line unit direction vector, in world coordinates 37 | * depthInterval Interval which contains the depth hypotheses 38 | * depthSamples Number of depth hypotheses 39 | */ 40 | Feature::Feature(const Mat& image_, const Rect& roi_, const Mat& R_, const Mat& t_, 41 | const Mat& dir_, const Point2d& depthInterval, int depthSamples) { 42 | 43 | image_.copyTo(image); 44 | roi = roi_; 45 | R_.copyTo(R); 46 | t_.copyTo(t); 47 | dir_.copyTo(dir); 48 | 49 | depths.resize(depthSamples); 50 | 51 | double step = (depthInterval.y - depthInterval.x) / (depthSamples - 1); 52 | 53 | for (int i = 0; i < depthSamples; i++) 54 | depths[i] = depthInterval.x + step * i; 55 | 56 | probs.resize(depthSamples, 1.0 / depthSamples); 57 | } -------------------------------------------------------------------------------- /MonoSLAM/feature.hpp: -------------------------------------------------------------------------------- 1 | #ifndef FEATURE_H 2 | #define FEATURE_H 3 | 4 | #include 5 | 6 | #include "opencv2/core/core.hpp" 7 | 8 | class Feature { 9 | 10 | public: 11 | 12 | cv::Mat image; // Original frame 13 | cv::Rect roi; // Patch region in the original frame 14 | cv::Mat pos; // Patch center, in world coordinates 15 | cv::Mat normal; // Patch normal vector, in world coordinates 16 | cv::Mat R; // Camera rotation in the original frame (world to camera) 17 | cv::Mat t; // Camera location in the original frame, in world coordinates 18 | 19 | cv::Mat P; // Feature state (position) covariance matrix 20 | 21 | int matchingFails; // Number of times the feature could not be matched 22 | int matchingAttempts; // Number of matching attempts 23 | 24 | cv::Mat dir; // Feature line unit direction vector, in world coordinates 25 | std::vector depths; // Feature depth hypotheses 26 | std::vector probs; // Hypotheses probabilities 27 | 28 | /* 29 | * Constructor for a fully initialized feature. It is a convenient method used only 30 | * during the map initialization step, when manually initialized features are added 31 | * with zero position uncertainty. 32 | * 33 | * image Camera frame at the time the feature is initialized 34 | * roi Feature patch 35 | * normal Patch normal vector 36 | * R Camera rotation (world to camera) at feature initialization 37 | * t Camera position, in world coordinates, at feature initialization 38 | */ 39 | Feature(const cv::Mat& image, const cv::Rect& roi, const cv::Mat& normal, 40 | const cv::Mat& R, const cv::Mat& t); 41 | 42 | /* 43 | * Constructor for a pre-initialized (candidate) feature. 44 | * 45 | * image Camera frame at the time the feature is initialized 46 | * roi Feature patch 47 | * R Camera rotation (world to camera) at feature initialization 48 | * t Camera position, in world coordinates, at feature initialization 49 | * dir Feature line unit direction vector, in world coordinates 50 | * depthInterval Interval which contains the depth hypotheses 51 | * depthSamples Number of depth hypotheses 52 | */ 53 | Feature(const cv::Mat& image, const cv::Rect& roi, const cv::Mat& R, const cv::Mat& t, 54 | const cv::Mat& dir, const cv::Point2d& depthInterval, int depthSamples); 55 | }; 56 | 57 | #endif -------------------------------------------------------------------------------- /MonoSLAM/camera.hpp: -------------------------------------------------------------------------------- 1 | #ifndef CAMERA_H 2 | #define CAMERA_H 3 | 4 | #include 5 | 6 | #include "opencv2/core/core.hpp" 7 | 8 | class Camera { 9 | 10 | public: 11 | 12 | cv::Mat r; // Position, in world coordinates 13 | cv::Mat q; // Orientation quaternion 14 | cv::Mat v; // Linear velocity, in world coordinates 15 | cv::Mat w; // Angular velocity, in camera coordinates 16 | 17 | cv::Mat P; // Camera state covariance matrix 18 | 19 | cv::Mat R; // Rotation matrix (from camera basis to world basis) 20 | cv::Mat K; // Intrinsic matrix 21 | cv::Mat distCoeffs; // Lens distortion coefficients 22 | 23 | cv::Size frameSize; // Frame size, in pixels 24 | 25 | /* 26 | * Constructor for a lens distortion camera model. 27 | */ 28 | Camera(const cv::Mat& K, const cv::Mat& distCoeffs, const cv::Size& frameSize); 29 | 30 | /* 31 | * Constructor for a pinhole camera model (distCoeffs is filled with zeros). 32 | */ 33 | Camera(const cv::Mat& K, const cv::Size& frameSize); 34 | 35 | /* 36 | * Warps a rectangular patch (presumed to be the projection of a certain region of a plane in space) 37 | * from one camera view to another. In particular, a map M is computed such that s2 = M * s1, where 38 | * s1 is the patch in the first view and s2 is the appearance of the same patch as would be seen from 39 | * the second view. A pinhole camera model is assumed (i.e. zero distortion coefficients) so that the 40 | * computed map be a planar homography. This is always a good approximation provided that the planar 41 | * patch is small or that the lens distortion is modest. 42 | * 43 | * p Point in the plane, in world coordinates 44 | * n Plane normal, in world coordinates 45 | * view1 Original view 46 | * patch1 Original patch 47 | * R1 First pose rotation (world to camera) 48 | * t1 First pose translation, in world coordinates 49 | * R2 Second pose rotation (world to camera) 50 | * t2 Second pose translation, in world coordinates 51 | * u First pixel coordinate of the projection of the original patch center 52 | * v Second pixel coordinate of the projection of the original patch center 53 | */ 54 | cv::Mat warpPatch(const cv::Mat& p, const cv::Mat& n, const cv::Mat& view1, const cv::Rect& patch1, 55 | const cv::Mat& R1, const cv::Mat& t1, const cv::Mat& R2, const cv::Mat& t2, 56 | int& u, int& v); 57 | 58 | /* 59 | * Projects 3D points onto the camera frame. 60 | * 61 | * R Rotation matrix (world to camera) 62 | * t Camera position, in world coordinates 63 | * points3D Points to be projected, in world coordinates 64 | * points2D Points projections, in pixels 65 | */ 66 | void projectPoints(const cv::Mat& R, const cv::Mat& t, const std::vector& points3D, 67 | std::vector& points2D); 68 | }; 69 | 70 | #endif -------------------------------------------------------------------------------- /MonoSLAM/quaternion.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include "quaternion.hpp" 4 | 5 | using namespace cv; 6 | 7 | /* 8 | * Builds a rotation quaternion from a (non-zero) rotation vector. 9 | */ 10 | Mat computeQuaternion(const Mat& rotv) { 11 | 12 | double x = rotv.at(0, 0); 13 | double y = rotv.at(1, 0); 14 | double z = rotv.at(2, 0); 15 | 16 | double angle = sqrt(x*x + y*y + z*z); 17 | 18 | double c = cos(0.5 * angle); 19 | double s = sin(0.5 * angle); 20 | 21 | double q[] = {c, s*x/angle, s*y/angle, s*z/angle}; 22 | 23 | return Mat(4, 1, CV_64FC1, q).clone(); 24 | } 25 | 26 | /* 27 | * Computes the Hamilton product of two quaternions. 28 | */ 29 | Mat quaternionMultiply(const Mat& q1, const Mat& q2) { 30 | 31 | double a1 = q1.at(0, 0); 32 | double b1 = q1.at(1, 0); 33 | double c1 = q1.at(2, 0); 34 | double d1 = q1.at(3, 0); 35 | 36 | double a2 = q2.at(0, 0); 37 | double b2 = q2.at(1, 0); 38 | double c2 = q2.at(2, 0); 39 | double d2 = q2.at(3, 0); 40 | 41 | double q[] = {a1*a2 - b1*b2 - c1*c2 - d1*d2, 42 | a1*b2 + b1*a2 + c1*d2 - d1*c2, 43 | a1*c2 - b1*d2 + c1*a2 + d1*b2, 44 | a1*d2 + b1*c2 - c1*b2 + d1*a2 45 | }; 46 | 47 | return Mat(4, 1, CV_64FC1, q).clone(); 48 | } 49 | 50 | /* 51 | * Computes a rotation matrix from a rotation quaternion. In particular, if the 52 | * quaternion is an orientation quaternion, which rotates a reference frame A to 53 | * its final pose B, the matrix is a change of basis from B to A. 54 | */ 55 | Mat getRotationMatrix(const Mat& q) { 56 | 57 | double a = q.at(0, 0); 58 | double b = q.at(1, 0); 59 | double c = q.at(2, 0); 60 | double d = q.at(3, 0); 61 | 62 | double m[] = {a*a+b*b-c*c-d*d, 2*b*c-2*a*d, 2*b*d+2*a*c, 63 | 2*b*c+2*a*d, a*a-b*b+c*c-d*d, 2*c*d-2*a*b, 64 | 2*b*d-2*a*c, 2*c*d+2*a*b, a*a-b*b-c*c+d*d 65 | }; 66 | 67 | return Mat(3, 3, CV_64FC1, m).clone(); 68 | } 69 | 70 | /* 71 | * Computes the inverse quaternion. 72 | */ 73 | Mat quaternionInv(const Mat& q) { 74 | 75 | Mat inv = Mat(4, 1, CV_64FC1); 76 | 77 | inv.at(0, 0) = q.at(0, 0); 78 | inv.at(1, 0) = -q.at(1, 0); 79 | inv.at(2, 0) = -q.at(2, 0); 80 | inv.at(3, 0) = -q.at(3, 0); 81 | 82 | return inv; 83 | } 84 | 85 | /* 86 | * Computes the matrices of the derivatives of R(q)^t with respect to the quaternion 87 | * components. That is, the entries of Ri are the derivatives of the entries of the 88 | * transpose of the matrix returned by getRotationMatrix(q) with respect to the ith 89 | * quaternion component. 90 | */ 91 | void getRotationMatrixDerivatives(const Mat& q, Mat& R1, Mat& R2, Mat& R3, Mat& R4) { 92 | 93 | double a = q.at(0, 0); 94 | double b = q.at(1, 0); 95 | double c = q.at(2, 0); 96 | double d = q.at(3, 0); 97 | 98 | double R1_[] = {a, d, -c, -d, a, b, c, -b, a}; 99 | R1 = 2 * Mat(3, 3, CV_64FC1, R1_).clone(); 100 | 101 | double R2_[] = {b, c, d, c, -b, a, d, -a, -b}; 102 | R2 = 2 * Mat(3, 3, CV_64FC1, R2_).clone(); 103 | 104 | double R3_[] = {-c, b, -a, b, c, d, a, d, -c}; 105 | R3 = 2 * Mat(3, 3, CV_64FC1, R3_).clone(); 106 | 107 | double R4_[] = {-d, a, b, -a, -d, c, b, c, d}; 108 | R4 = 2 * Mat(3, 3, CV_64FC1, R4_).clone(); 109 | } -------------------------------------------------------------------------------- /MonoSLAM/camera.cpp: -------------------------------------------------------------------------------- 1 | #include "opencv2/imgproc/imgproc.hpp" 2 | 3 | #include "camera.hpp" 4 | #include "util.hpp" 5 | 6 | using namespace cv; 7 | using namespace std; 8 | 9 | /* 10 | * Constructor for a lens distortion camera model. 11 | */ 12 | Camera::Camera(const Mat& K_, const Mat& distCoeffs_, const Size& frameSize_) { 13 | 14 | K_.copyTo(K); 15 | frameSize = frameSize_; 16 | 17 | distCoeffs_.copyTo(distCoeffs); 18 | } 19 | 20 | /* 21 | * Constructor for a pinhole camera model (distCoeffs is filled with zeros). 22 | */ 23 | Camera::Camera(const Mat& K_, const Size& frameSize_) { 24 | 25 | K_.copyTo(K); 26 | frameSize = frameSize_; 27 | 28 | distCoeffs = Mat::zeros(1, 5, CV_64FC1); 29 | } 30 | 31 | /* 32 | * Warps a rectangular patch (presumed to be the projection of a certain region of a plane in space) 33 | * from one camera view to another. In particular, a map M is computed such that s2 = M * s1, where 34 | * s1 is the patch in the first view and s2 is the appearance of the same patch as would be seen from 35 | * the second view. A pinhole camera model is assumed (i.e. zero distortion coefficients) so that the 36 | * computed map be a planar homography. This is always a good approximation provided that the planar 37 | * patch is small or that the lens distortion is modest. 38 | * 39 | * p Point in the plane, in world coordinates 40 | * n Plane normal, in world coordinates 41 | * view1 Original view 42 | * patch1 Original patch 43 | * R1 First pose rotation (world to camera) 44 | * t1 First pose translation, in world coordinates 45 | * R2 Second pose rotation (world to camera) 46 | * t2 Second pose translation, in world coordinates 47 | * u First pixel coordinate of the projection of the original patch center 48 | * v Second pixel coordinate of the projection of the original patch center 49 | */ 50 | Mat Camera::warpPatch(const Mat& p, const Mat& n, const Mat& view1, const Rect& patch1, 51 | const Mat& R1, const Mat& t1, const Mat& R2, const Mat& t2, 52 | int& u, int& v) { 53 | 54 | // Black background 55 | Mat black(view1.rows, view1.cols, view1.type(), Scalar(0)); 56 | 57 | // Copy the original patch into the black background 58 | view1(patch1).copyTo(black(patch1)); 59 | 60 | // Compute the homography matrix between the first and second views 61 | Mat H = computeHomography(p, n, K, R1, t1, R2, t2); 62 | 63 | // Apply the homography 64 | Mat warped; 65 | warpPerspective(black, warped, H, view1.size()); 66 | 67 | // Set every non-black pixel of the warped image to white 68 | Mat binary; 69 | threshold(warped, binary, 0, 255, THRESH_BINARY); 70 | 71 | // Use the binary mask to crop the warped image so that it only contains the patch 72 | Mat points; 73 | findNonZero(binary, points); 74 | Rect patchBox = boundingRect(points); 75 | 76 | // Compute the projection of the center of the patch 77 | int x = patch1.x + patch1.width / 2; 78 | int y = patch1.y + patch1.height / 2; 79 | 80 | Mat centerProj = H * (Mat_(3, 1) << x, y, 1); 81 | centerProj /= centerProj.at(2, 0); 82 | 83 | u = centerProj.at(0, 0) - patchBox.x; 84 | v = centerProj.at(1, 0) - patchBox.y; 85 | 86 | return warped(patchBox); 87 | } 88 | 89 | /* 90 | * Projects 3D points onto the camera frame. 91 | * 92 | * R Rotation matrix (world to camera) 93 | * t Camera position, in world coordinates 94 | * points3D Points to be projected, in world coordinates 95 | * points2D Points projections, in pixels 96 | */ 97 | void Camera::projectPoints(const Mat& R, const Mat& t, const vector& points3D, 98 | vector& points2D) { 99 | 100 | double fx = K.at(0, 0); 101 | double fy = K.at(1, 1); 102 | double cx = K.at(0, 2); 103 | double cy = K.at(1, 2); 104 | 105 | double k1 = distCoeffs.at(0, 0); 106 | double k2 = distCoeffs.at(0, 1); 107 | double p1 = distCoeffs.at(0, 2); 108 | double p2 = distCoeffs.at(0, 3); 109 | double k3 = distCoeffs.at(0, 4); 110 | 111 | points2D.resize(points3D.size()); 112 | 113 | for (unsigned int i = 0; i < points3D.size(); i++) { 114 | 115 | Mat pCam = R * (Mat(points3D[i]) - t); 116 | 117 | double xn = pCam.at(0, 0) / pCam.at(2, 0); 118 | double yn = pCam.at(1, 0) / pCam.at(2, 0); 119 | 120 | double r2 = xn*xn + yn*yn; 121 | double r4 = r2 * r2; 122 | double r6 = r4 * r2; 123 | 124 | double u = xn * (1 + k1*r2 + k2*r4 + k3*r6) + 2*p1*xn*yn + p2*(r2 + 2*xn*xn); 125 | double v = yn * (1 + k1*r2 + k2*r4 + k3*r6) + 2*p2*xn*yn + p1*(r2 + 2*yn*yn); 126 | 127 | u = fx*u + cx; 128 | v = fy*v + cy; 129 | 130 | points2D[i] = Point2d(u, v); 131 | } 132 | } -------------------------------------------------------------------------------- /MonoSLAM/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #include "opencv2/calib3d/calib3d.hpp" 7 | #include "opencv2/highgui/highgui.hpp" 8 | #include "opencv2/imgproc/imgproc.hpp" 9 | 10 | #include "camera.hpp" 11 | #include "feature.hpp" 12 | #include "map.hpp" 13 | #include "quaternion.hpp" 14 | #include "util.hpp" 15 | 16 | using namespace cv; 17 | using namespace std; 18 | 19 | typedef chrono::duration> seconds_; 20 | typedef chrono::high_resolution_clock clock_; 21 | 22 | bool getOptions(int argc, char *argv[], string& cameraFile, int& patternRows, int& patternCols, 23 | double& squareSize); 24 | void printUsage(const char *execName); 25 | 26 | void readFrame(VideoCapture& cap, Mat& frame); 27 | 28 | int main(int argc, char *argv[]) { 29 | 30 | string cameraFile; 31 | int patternRows, patternCols; 32 | double squareSize; 33 | 34 | if (!getOptions(argc, argv, cameraFile, patternRows, patternCols, squareSize)) { 35 | 36 | printUsage(argv[0]); 37 | return -1; 38 | } 39 | 40 | FileStorage fs(cameraFile, FileStorage::READ); 41 | 42 | if (!fs.isOpened()) { 43 | 44 | cerr << "Could not open \"" << cameraFile << "\": camera settings not loaded" << endl; 45 | return -1; 46 | } 47 | 48 | Size frameSize; 49 | Mat K, distCoeffs; 50 | 51 | fs["Image_Size"] >> frameSize; 52 | fs["Intrinsic_Matrix"] >> K; 53 | fs["Distortion_Coefficients"] >> distCoeffs; 54 | 55 | fs.release(); 56 | 57 | int patchSize = 22; 58 | int minDensity = 8; 59 | int maxDensity = 18; 60 | double failTolerance = 0.5; 61 | Mat accelerationVariances = (Mat_(6, 1) << 0.025, 0.025, 0.025, 0.6, 0.6, 0.6); 62 | Mat measurementNoiseVariances = (Mat_(2, 1) << 9, 9); 63 | 64 | // Build new map 65 | Map map(K, distCoeffs, frameSize, patchSize, minDensity, maxDensity, failTolerance, 66 | accelerationVariances, measurementNoiseVariances); 67 | 68 | // Chessboard size 69 | Size patternSize(patternCols - 1, patternRows - 1); 70 | 71 | // Camera initial state variances 72 | vector var({0.0001, 0.0001, 0.0001, 73 | 0., 0., 0., 0., 74 | 0.0001, 0.0001, 0.0001, 75 | 0.008, 0.008, 0.008, 76 | }); 77 | 78 | // Initialize video feed device 79 | VideoCapture cap(0); 80 | 81 | if (!cap.isOpened()) 82 | return -1; 83 | 84 | // Create a window 85 | string window = "MonoSLAM"; 86 | namedWindow(window, CV_WINDOW_AUTOSIZE); 87 | 88 | Mat frame, gray; 89 | 90 | bool init; 91 | 92 | chrono::time_point t0; 93 | 94 | for (;;) { 95 | 96 | readFrame(cap, frame); 97 | 98 | t0 = clock_::now(); 99 | 100 | imshow(window, frame); 101 | 102 | cvtColor(frame, gray, CV_BGR2GRAY); 103 | 104 | if ((init = map.initMap(gray, patternSize, squareSize, var))) 105 | break; 106 | 107 | if (waitKey(1) == 27) 108 | break; 109 | } 110 | 111 | if (!init) 112 | return 0; 113 | 114 | chrono::time_point t1; 115 | 116 | for (;;) { 117 | 118 | cout << "Visible features: " 119 | << map.numVisibleFeatures << "/" << map.features.size() << endl; 120 | 121 | map.trackNewCandidates(gray); 122 | 123 | readFrame(cap, frame); 124 | 125 | t1 = clock_::now(); 126 | double dt = chrono::duration_cast(t1 - t0).count(); 127 | t0 = t1; 128 | 129 | cvtColor(frame, gray, CV_BGR2GRAY); 130 | cvtColor(gray, frame, CV_GRAY2RGB); 131 | 132 | map.predict(dt); 133 | map.update(gray, frame); 134 | 135 | imshow(window, frame); 136 | 137 | if (waitKey(1) == 27) 138 | break; 139 | } 140 | } 141 | 142 | bool getOptions(int argc, char *argv[], string& cameraFile, int& patternRows, int& patternCols, 143 | double& squareSize) { 144 | 145 | // Look for the help option 146 | for (int i = 1; i < argc; i++) 147 | if (string(argv[i]) == "-h") 148 | return false; 149 | 150 | // Check the number of input options 151 | if (argc != 5) 152 | return false; 153 | 154 | cameraFile = argv[1]; 155 | 156 | patternRows = (int) strtol(argv[2], NULL, 10); 157 | if (patternRows == 0 || errno == ERANGE) return false; 158 | 159 | patternCols = (int) strtol(argv[3], NULL, 10); 160 | if (patternCols == 0 || errno == ERANGE) return false; 161 | 162 | squareSize = strtod(argv[4], NULL); 163 | if (squareSize == 0 || errno == ERANGE) return false; 164 | 165 | return true; 166 | } 167 | 168 | void printUsage(const char *execName) { 169 | 170 | cerr << "Usage: " << execName << " cameraFile patternRows patternCols squareSize (-h for help)" << endl; 171 | } 172 | 173 | void readFrame(VideoCapture& cap, Mat& frame) { 174 | 175 | chrono::time_point t0; 176 | double elapsed; 177 | 178 | double tol = 1 / double(30); 179 | 180 | do { 181 | 182 | t0 = clock_::now(); 183 | cap.grab(); 184 | elapsed = chrono::duration_cast(clock_::now() - t0).count(); 185 | 186 | } while (elapsed < tol); 187 | 188 | cap.retrieve(frame); 189 | } -------------------------------------------------------------------------------- /MonoSLAM/util.hpp: -------------------------------------------------------------------------------- 1 | #ifndef UTIL_H 2 | #define UTIL_H 3 | 4 | #include "opencv2/core/core.hpp" 5 | 6 | constexpr double PI = 3.1415926535897932385; 7 | constexpr double PI_DOUBLE = 6.2831853071795864769; 8 | constexpr double RAD_TO_DEG = 57.295779513082320877; 9 | constexpr double DEG_TO_RAD = 0.0174532925199432957; 10 | 11 | /* 12 | * Computes the homography matrix H between two pinhole camera views of a plane. 13 | * In particular, p2 = H * p1, where p1 is the pixel in the first view which 14 | * corresponds to a point p in the plane, and p2 is the pixel which corresponds 15 | * to p in the second view. 16 | * 17 | * p Point in the plane, in world coordinates 18 | * n Plane normal, in world coordinates 19 | * K Camera intrinsic matrix 20 | * R1 First pose rotation (world to camera) 21 | * t1 First pose translation, in world coordinates 22 | * R2 Second pose rotation (world to camera) 23 | * t2 Second pose translation, in world coordinates 24 | */ 25 | cv::Mat computeHomography(const cv::Mat& p, const cv::Mat& n, const cv::Mat& K, 26 | const cv::Mat& R1, const cv::Mat& t1, const cv::Mat& R2, const cv::Mat& t2); 27 | 28 | /* 29 | * Returns a square region of interest given its center pixel coordinates and size. 30 | * Note that only if size is an odd number will the square be truly centered at the 31 | * desired position. 32 | * 33 | * center Center of the square 34 | * size Size of the square 35 | */ 36 | cv::Rect buildSquare(const cv::Point2i& center, int size); 37 | 38 | /* 39 | * Removes the nth row and column of a square matrix. The matrix is not reduced 40 | * in-place, but a deep copy is returned. 41 | * 42 | * mat Matrix to be reduced 43 | * idx Row and column to be deleted 44 | */ 45 | cv::Mat removeRowCol(const cv::Mat& mat, int idx); 46 | 47 | /* 48 | * Removes specific rows and columns of a square matrix. The matrix is not reduced in-place, 49 | * but a deep copy is returned. The indices to be removed should be sorted in ascending order. 50 | * The stride indicates how the matrix rows and columns are grouped together and referenced by 51 | * the indices. For example, given indices 0, 3, 7 and a stride of 2 the deleted rows and columns 52 | * are 0, 1, 6, 7, 14, 15. 53 | * 54 | * mat Matrix to be reduced 55 | * indices Sorted rows and columns to be deleted 56 | * stride Row and column stride 57 | */ 58 | cv::Mat removeRowsCols(const cv::Mat& mat, std::vector& indices, int stride = 1); 59 | 60 | /* 61 | * Removes the nth row of a matrix. The matrix is not reduced in-place, but a deep 62 | * copy is returned. 63 | * 64 | * mat Matrix to be reduced 65 | * idx Row to be deleted 66 | */ 67 | cv::Mat removeRow(const cv::Mat& mat, int idx); 68 | 69 | /* 70 | * Removes specific rows of a matrix. The matrix is not reduced in-place, but a deep 71 | * copy is returned. The indices to be removed should be sorted in ascending order. 72 | * The stride indicates how the matrix rows are grouped together and referenced by the 73 | * indices. For example, given indices 0, 3, 7 and a stride of 2 the deleted rows are 74 | * 0, 1, 6, 7, 14, 15. 75 | * 76 | * mat Matrix to be reduced 77 | * indices Sorted rows to be deleted 78 | * stride Row stride 79 | */ 80 | cv::Mat removeRows(const cv::Mat& mat, std::vector& indices, int stride = 1); 81 | 82 | /* 83 | * Removes specific elements of a vector. The vector is reduced in-place. The indices of the 84 | * elements to be removed should be sorted in ascending order. 85 | * 86 | * vec Vector to be reduced 87 | * indices Sorted indices of elements to be deleted 88 | */ 89 | template 90 | void removeIndices(std::vector& vec, const std::vector& indices) { 91 | 92 | for (int i = indices.size() - 1; i >= 0; i--) 93 | vec.erase(vec.begin() + indices[i]); 94 | } 95 | 96 | /* 97 | * Returns the ellipses where the predicted in sight features should be found with 98 | * high probability. In particular, the ellipses are constructed so that they are 99 | * confidence regions at level 0.86. 100 | * 101 | * means Predicted in sight features pixel locations 102 | * S Innovation covariance matrix of the predicted in sight features 103 | */ 104 | std::vector computeEllipses(const std::vector& means, const cv::Mat& S); 105 | 106 | /* 107 | * Returns the smallest straight rectangle which contains the given ellipse. The computed 108 | * rectangle is cropped if it exceeds the given image boundary so that it fits inside it. 109 | * 110 | * ellipse Ellipse to be bounded 111 | * imageSize Size of the image 112 | */ 113 | cv::Rect getBoundingBox(const cv::RotatedRect& ellipse, const cv::Size& imageSize); 114 | 115 | /* 116 | * Draws a rectangle on the given image. The image is modified by the function so a deep 117 | * copy should be made in order to preserve the original. 118 | * 119 | * image Grayscale or color image to draw on 120 | * rectangle Rectangle 121 | * color Color of the rectangle 122 | */ 123 | void drawRectangle(cv::Mat& image, const cv::Rect& rectangle, const cv::Scalar& color); 124 | 125 | /* 126 | * Draws a square on the given image. The image is modified by the function so a deep 127 | * copy should be made in order to preserve the original. 128 | * 129 | * image Grayscale or color image to draw on 130 | * center Center of the square 131 | * width Width of the square 132 | * color Color of the square 133 | */ 134 | void drawSquare(cv::Mat& image, const cv::Point2i& center, int width, const cv::Scalar& color); 135 | 136 | /* 137 | * Draws an ellipse on the given image. The image is modified by the function so a deep 138 | * copy should be made in order to preserve the original. 139 | * 140 | * image Grayscale or color image to draw on 141 | * ellipse Ellipse 142 | * color Color of the ellipse 143 | */ 144 | void drawEllipse(cv::Mat& image, const cv::RotatedRect& ellipse, const cv::Scalar& color); 145 | 146 | /* 147 | * Draws a circle on the given image. The image is modified by the function so a deep 148 | * copy should be made in order to preserve the original. 149 | * 150 | * image Grayscale or color image to draw on 151 | * center Center of the circle 152 | * radius Radius of the circle 153 | * color Color of the circle 154 | */ 155 | void drawCircle(cv::Mat& image, const cv::Point2i& center, int radius, const cv::Scalar& color); 156 | 157 | void drawTemplate(cv::Mat& image, const cv::Mat& templ, const cv::Point2d& position, int cx, int cy); 158 | 159 | #endif -------------------------------------------------------------------------------- /Calibration/calibration.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "opencv2/calib3d/calib3d.hpp" 6 | #include "opencv2/highgui/highgui.hpp" 7 | #include "opencv2/imgproc/imgproc.hpp" 8 | 9 | using namespace cv; 10 | using namespace std; 11 | 12 | typedef chrono::high_resolution_clock clock_; 13 | typedef chrono::duration> second_; 14 | 15 | bool getOptions(int argc, char *argv[], int& rows, int& cols, double& squareSize, double& tol, 16 | string& filename); 17 | void printUsage(const char *execName); 18 | 19 | vector computeObjPoints(Size patternSize, double squareSize); 20 | bool addCornerPoints(const Mat& image, int rows, int cols, double squareSize, 21 | vector>& imagePoints, 22 | vector>& objectPoints); 23 | 24 | int main(int argc, char *argv[]) { 25 | 26 | int rows, cols; 27 | double squareSize, tol; 28 | string filename; 29 | 30 | tol = 1.5; // Default sample reprojection error tolerance 31 | 32 | if (!getOptions(argc, argv, rows, cols, squareSize, tol, filename)) { 33 | 34 | printUsage(argv[0]); 35 | return -1; 36 | } 37 | 38 | filename += string(".xml"); 39 | 40 | cout << "Calibrating with a " << cols << "x" << rows 41 | << " chessboard with a square size of " << squareSize << " meters" << endl 42 | << "Sample reprojection error tolerance is set to " << tol << " pixels" << endl; 43 | 44 | // Initialize video feed device 45 | VideoCapture cap(0); 46 | 47 | if (!cap.isOpened()) 48 | return -1; 49 | 50 | // Create a window 51 | string window = "Camera Calibration"; 52 | namedWindow(window, CV_WINDOW_AUTOSIZE); 53 | 54 | Mat frame, patternFrame; 55 | vector> imagePoints; 56 | vector> objectPoints; 57 | 58 | // Create the image to be shown 59 | cap.read(frame); 60 | Mat showFrame(frame.rows, 2*frame.cols, frame.type()); 61 | 62 | int i = 0; 63 | chrono::time_point t0 = clock_::now(); 64 | 65 | while (true) { 66 | 67 | cap.read(frame); 68 | 69 | frame.copyTo(showFrame(Rect(0, 0, frame.cols, frame.rows))); 70 | 71 | double elapsed = chrono::duration_cast(clock_::now() - t0).count(); 72 | 73 | // Leave some seconds between each pattern capture 74 | if (elapsed > 2) { 75 | 76 | frame.copyTo(patternFrame); 77 | 78 | if (addCornerPoints(patternFrame, rows, cols, squareSize, imagePoints, objectPoints)) { 79 | 80 | patternFrame.copyTo(showFrame(Rect(frame.cols-1, 0, frame.cols, frame.rows))); 81 | cout << "\r" << ++i << " samples captured" << flush; 82 | t0 = clock_::now(); 83 | } 84 | } 85 | 86 | imshow(window, showFrame); 87 | 88 | if (waitKey(1) == 27) 89 | break; 90 | } 91 | 92 | if (i == 0) { 93 | 94 | cout << "Calibration canceled: no samples to work with" << endl; 95 | return -1; 96 | } 97 | 98 | cout << endl; 99 | 100 | Size imageSize = frame.size(); 101 | Mat cameraMat, distCoeffs, stdDevIntrinsics, stdDevExtrinsics, perViewErrors; 102 | vector rvecs, tvecs; 103 | 104 | double rmse = calibrateCamera(objectPoints, imagePoints, imageSize, cameraMat, distCoeffs, rvecs, tvecs, 105 | stdDevIntrinsics, stdDevExtrinsics, perViewErrors); 106 | 107 | vector> newImagePoints; 108 | vector> newObjectPoints; 109 | 110 | // Remove samples with high reprojection error 111 | for (int i = 0; i < perViewErrors.rows; i++) { 112 | 113 | if (perViewErrors.at(i, 0) < tol) { 114 | 115 | newImagePoints.push_back(imagePoints[i]); 116 | newObjectPoints.push_back(objectPoints[i]); 117 | } 118 | } 119 | 120 | if (newImagePoints.empty()) { 121 | 122 | cout << "Calibration canceled: no good samples to work with" << endl; 123 | return -1; 124 | } 125 | 126 | if (newImagePoints.size() < imagePoints.size()) { 127 | 128 | // Recalibrate with good samples 129 | rmse = calibrateCamera(newObjectPoints, newImagePoints, imageSize, cameraMat, distCoeffs, rvecs, tvecs, 130 | stdDevIntrinsics, stdDevExtrinsics, perViewErrors); 131 | 132 | cout << "Camera calibrated using only " << newImagePoints.size() << " samples" << endl; 133 | } 134 | 135 | cout << "Intrinsic matrix:" << endl 136 | << cameraMat << endl 137 | << "Distortion coeficients: " << distCoeffs << endl 138 | << "RMS reprojection error (pixels): " << rmse << endl; 139 | 140 | FileStorage fs(filename, FileStorage::WRITE); 141 | 142 | if (!fs.isOpened()) { 143 | 144 | cout << "Could not open \"" << filename << "\": camera settings not saved" << endl; 145 | return -1; 146 | } 147 | 148 | fs << "Image_Size" << imageSize; 149 | fs << "Intrinsic_Matrix" << cameraMat; 150 | fs << "Distortion_Coefficients" << distCoeffs; 151 | 152 | cout << "Camera settings saved to \"" << filename << "\"" << endl; 153 | } 154 | 155 | bool getOptions(int argc, char *argv[], int& rows, int& cols, double& squareSize, double& tol, 156 | string& filename) { 157 | 158 | // Look for the help option 159 | for (int i = 1; i < argc; i++) 160 | if (string(argv[i]) == "-h" || string(argv[i]) == "--help") 161 | return false; 162 | 163 | // Check the number of input options 164 | if (argc != 5 && argc != 6) 165 | return false; 166 | 167 | rows = (int) strtol(argv[1], NULL, 10); 168 | if (rows == 0 || errno == ERANGE) return false; 169 | 170 | cols = (int) strtol(argv[2], NULL, 10); 171 | if (cols == 0 || errno == ERANGE) return false; 172 | 173 | squareSize = strtod(argv[3], NULL); 174 | if (squareSize == 0 || errno == ERANGE) return false; 175 | 176 | if (argc == 6) { 177 | 178 | tol = strtod(argv[4], NULL); 179 | if (tol == 0 || errno == ERANGE) return false; 180 | } 181 | 182 | filename = (argc == 6) ? argv[5] : argv[4]; 183 | 184 | return true; 185 | } 186 | 187 | void printUsage(const char *execName) { 188 | 189 | cerr << "Usage: " << execName << " rows cols square_size [tol] filename (-h, --help for help)" << endl; 190 | } 191 | 192 | vector computeObjPoints(Size patternSize, double squareSize) { 193 | 194 | vector objPoints; 195 | 196 | for (int i = 0; i < patternSize.height; i++) { 197 | 198 | for (int j = 0; j < patternSize.width; j++) { 199 | 200 | objPoints.push_back(Point3f(squareSize * j, squareSize * i, 0)); 201 | } 202 | } 203 | 204 | return objPoints; 205 | } 206 | 207 | bool addCornerPoints(const Mat& image, int rows, int cols, double squareSize, 208 | vector>& imagePoints, 209 | vector>& objectPoints) { 210 | 211 | int pointsPerRow = cols - 1; 212 | int pointsPerCol = rows - 1; 213 | 214 | Size patternSize(pointsPerRow, pointsPerCol); 215 | 216 | vector imgPoints; 217 | 218 | bool patternFound = findChessboardCorners(image, patternSize, imgPoints, 219 | CALIB_CB_ADAPTIVE_THRESH + 220 | CALIB_CB_NORMALIZE_IMAGE + 221 | CALIB_CB_FAST_CHECK); 222 | 223 | if (patternFound) { 224 | 225 | Mat gray; 226 | 227 | cvtColor(image, gray, CV_BGR2GRAY); 228 | 229 | cornerSubPix(gray, imgPoints, Size(11, 11), Size(-1, -1), 230 | TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); 231 | 232 | imagePoints.push_back(imgPoints); 233 | objectPoints.push_back(computeObjPoints(patternSize, squareSize)); 234 | 235 | drawChessboardCorners(image, patternSize, imgPoints, patternFound); 236 | } 237 | 238 | return patternFound; 239 | } -------------------------------------------------------------------------------- /MonoSLAM/map.hpp: -------------------------------------------------------------------------------- 1 | #ifndef MAP_H 2 | #define MAP_H 3 | 4 | #include 5 | 6 | #include "opencv2/core/core.hpp" 7 | 8 | #include "camera.hpp" 9 | #include "feature.hpp" 10 | 11 | class Map { 12 | 13 | public: 14 | 15 | Camera camera; // Camera 16 | 17 | std::vector features; // Initialized map features (used for tracking) 18 | std::vector candidates; // Pre-initialized features (updated by particle filter) 19 | 20 | int patchSize; // Size of the feature planar patch, in pixels 21 | 22 | int minFeatureDensity; // Minimum number of features per frame 23 | int maxFeatureDensity; // Maximum number of features per frame 24 | 25 | double failTolerance; // Maximum ratio of matching failures before feature rejection 26 | 27 | int numVisibleFeatures; // Number of currently visible features (a subset of those in sight) 28 | std::vector inviewPos; // Image positions of currently (predicted) in sight features 29 | 30 | cv::Mat x; // Map state (camera and features states) 31 | cv::Mat P; // Map state covariance matrix 32 | 33 | cv::Mat A; // Linear and angular accelerations noise covariance matrix 34 | cv::Mat R; // Measurement noise variances, in pixels 35 | 36 | /* 37 | * Constructor for a lens distortion camera model. 38 | */ 39 | Map(const cv::Mat& K, 40 | const cv::Mat& distCoeffs, 41 | const cv::Size& frameSize, 42 | int patchSize_, 43 | int minFeatureDensity_, 44 | int maxFeatureDensity_, 45 | double failTolerance_, 46 | const cv::Mat& accelerationVariances, 47 | const cv::Mat& measurementNoiseVariances) : 48 | camera(K, distCoeffs, frameSize), 49 | patchSize(patchSize_), 50 | minFeatureDensity(minFeatureDensity_), 51 | maxFeatureDensity(maxFeatureDensity_), 52 | failTolerance(failTolerance_), 53 | numVisibleFeatures(0), 54 | x(13, 1, CV_64FC1, cv::Scalar(0)), 55 | P(13, 13, CV_64FC1, cv::Scalar(0)) { 56 | 57 | A = cv::Mat::diag(accelerationVariances); 58 | measurementNoiseVariances.copyTo(R); 59 | } 60 | 61 | /* 62 | * Constructor for a pinhole camera model. 63 | */ 64 | Map(const cv::Mat& K, 65 | const cv::Size& frameSize, 66 | int patchSize_, 67 | int minFeatureDensity_, 68 | int maxFeatureDensity_, 69 | double failTolerance_, 70 | const cv::Mat& accelerationVariances, 71 | const cv::Mat& measurementNoiseVariances) : 72 | camera(K, frameSize), 73 | patchSize(patchSize_), 74 | minFeatureDensity(minFeatureDensity_), 75 | maxFeatureDensity(maxFeatureDensity_), 76 | failTolerance(failTolerance_), 77 | numVisibleFeatures(0), 78 | x(13, 1, CV_64FC1, cv::Scalar(0)), 79 | P(13, 13, CV_64FC1, cv::Scalar(0)) { 80 | 81 | A = cv::Mat::diag(accelerationVariances); 82 | measurementNoiseVariances.copyTo(R); 83 | } 84 | 85 | /* 86 | * Initializes the map by detecting a known chessboard pattern. It provides an 87 | * absolute scale reference, some manually initialized features to track, and 88 | * the world frame origin and initial camera pose. The world frame origin is 89 | * positioned at the top left corner of the pattern, with the x axis pointing 90 | * along the pattern height and the y axis pointing along the pattern width. 91 | * Note that the z axis is determined by the right-hand rule (see solvePnP). 92 | * It is assumed the camera has zero linear and angular velocities. 93 | * 94 | * frame Grayscale frame 95 | * patternSize Pattern dimensions (see findChessboardCorners) 96 | * squareSize Size of a chessboard square, in meters 97 | * var Variances of the initial camera state components 98 | */ 99 | bool initMap(const cv::Mat& frame, const cv::Size& patternSize, double squareSize, 100 | const std::vector& var); 101 | 102 | /* 103 | * Updates the camera and features data with the map state vector and map state 104 | * covariance matrix data. Only shallow copies are performed. This function should 105 | * be called whenever the current state of the system must be broadcast to each 106 | * of its parts. This method exists because the map state (x and P) is continuously 107 | * updated by the EKF and these changes will not be directly reflected in the camera 108 | * and features instances. 109 | */ 110 | void broadcastData(); 111 | 112 | /* 113 | * Detects corners and initializes new feature candidates whenever there are not 114 | * enough visible features and no candidates are being tracked. Returns a boolean 115 | * indicating whether new candidates were detected or not. 116 | * 117 | * frame Grayscale frame 118 | */ 119 | bool trackNewCandidates(const cv::Mat& frame); 120 | 121 | /* 122 | * Applies the first step of the Extended Kalman Filter. In particular, given the 123 | * current state estimate x_k|k and the current state covariance matrix P_k|k, it 124 | * computes the next a priori state estimate x_k+1|k (via the camera motion model) 125 | * and its associated covariance matrix P_k+1|k. 126 | * 127 | * dt Time interval between the current and past frames 128 | */ 129 | void predict(double dt); 130 | 131 | /* 132 | * Applies the update step of the Extended Kalman Filter. In particular, given the 133 | * predicted (a priori) state estimate x_k+1|k and the associated covariance matrix 134 | * P_k+1|k, it computes the corrected (a posteriori) estimate x_k+1|k+1 and its 135 | * covariance P_k+1|k+1 by measuring known, currently visible map features. 136 | * 137 | * gray Grayscale frame, used for feature matching 138 | * frame Color frame, used as a canvas to draw features on 139 | */ 140 | void update(const cv::Mat& gray, cv::Mat& frame); 141 | 142 | private: 143 | 144 | /* 145 | * Adds an initial feature to the map. Its position is known with zero uncertainty 146 | * hence the associated covariance matrix is the zero matrix. Moreover, since the 147 | * feature patch lies on the chessboard pattern its normal vector is taken to be 148 | * the z axis unit vector. The feature is also set as visible. If the feature patch 149 | * exceeds the frame boundaries the feature is not initialized and false is returned. 150 | * 151 | * frame Grayscale frame 152 | * pos2D Feature position in the image, in pixels 153 | * pos3D Feature position, in world coordinates 154 | * R Camera rotation (from world to camera) 155 | * t Camera position, in world coordinates 156 | */ 157 | bool addInitialFeature(const cv::Mat& frame, const cv::Point2f& pos2D, const cv::Point3f& pos3D, 158 | const cv::Mat& R, const cv::Mat& t); 159 | 160 | /* 161 | * Sets the map as it was upon creation. 162 | */ 163 | void reset(); 164 | 165 | /* 166 | * Returns corners found by the Shi-Tomasi corner detector. In particular, the 167 | * number of computed corners is such that when the associated pre-initialized 168 | * features are added to the map for tracking, the number of visible features 169 | * in the current frame will (hopefully) be the maximum allowed. 170 | * 171 | * frame Grayscale frame 172 | */ 173 | cv::Mat findCorners(const cv::Mat& frame); 174 | 175 | /* 176 | * Applies the state transition function to the current state estimate. It is mandatory 177 | * that this method be called after computeProcessMatrix and computeProcessNoiseMatrix 178 | * since the latter expect the last a posteriori (corrected) state estimate in the state 179 | * vector x. 180 | * 181 | * dt Time interval between the current and past frames 182 | */ 183 | void applyMotionModel(double dt); 184 | 185 | /* 186 | * Returns the Jacobian matrix of the state transition function with respect to the 187 | * complete state (r, q, v, w, f1, ..., fn). This matrix is evaluated at the current 188 | * state estimate on the assumption that there is zero process noise. This method must 189 | * be called before applyMotionModel since the latter updates the current estimate x. 190 | * 191 | * dt Time interval between the current and past frames 192 | */ 193 | cv::Mat computeProcessMatrix(double dt); 194 | 195 | /* 196 | * Returns the product W * Q * W^t, where Q is the process noise covariance matrix 197 | * and W is the Jacobian matrix of the state transition function with respect to 198 | * the noise vector (V1, V2, V3, W1, W2, W3). The latter is evaluated at the current 199 | * state estimate on the assumption that there is zero process noise. This method must 200 | * be called before applyMotionModel since the latter updates the current estimate x. 201 | * 202 | * dt Time interval between the current and past frames 203 | * F Jacobian matrix of the state transition function with respect to x 204 | */ 205 | cv::Mat computeProcessNoiseMatrix(double dt, const cv::Mat& F); 206 | 207 | /* 208 | * Returns the Jacobian matrix of the features measurement function with respect 209 | * to the complete state (r, q, v, w, f1, ..., fn). This matrix is evaluated at 210 | * the predicted (a priori) state estimate, therefore it should only be called 211 | * after applyMotionModel since the latter updates the current estimate x. Also, 212 | * it updates the vector of predicted insight features pixel positions, inviewPos, 213 | * and populates a vector with the predicted in sight features indices. 214 | */ 215 | cv::Mat computeMeasurementMatrix(std::vector& inviewIndices); 216 | 217 | void drawFeatures(cv::Mat& frame, 218 | const std::vector& matchedInviewIndices, const std::vector& failedInviewIndices, 219 | const std::vector& ellipses, bool drawEllipses = true); 220 | 221 | void removeBadFeatures(const std::vector& failedInviewIndices, const std::vector& failedIndices); 222 | 223 | /* 224 | * Normalizes the system state orientation quaternion and updates the covariance 225 | * matrix accordingly by first-order error propagation. It is mandatory that the 226 | * function be called after the EKF update step since the corrected (a posteriori) 227 | * orientation quaternion might not be a unit quaternion. 228 | */ 229 | void renormalizeQuaternion(); 230 | }; 231 | 232 | #endif -------------------------------------------------------------------------------- /MonoSLAM/util.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "opencv2/imgproc/imgproc.hpp" 5 | 6 | #include "util.hpp" 7 | 8 | using namespace cv; 9 | using namespace std; 10 | 11 | /* 12 | * Computes the homography matrix H between two pinhole camera views of a plane. 13 | * In particular, p2 = H * p1, where p1 is the pixel in the first view which 14 | * corresponds to a point p in the plane, and p2 is the pixel which corresponds 15 | * to p in the second view. 16 | * 17 | * p Point in the plane, in world coordinates 18 | * n Plane normal, in world coordinates 19 | * K Camera intrinsic matrix 20 | * R1 First pose rotation (world to camera) 21 | * t1 First pose translation, in world coordinates 22 | * R2 Second pose rotation (world to camera) 23 | * t2 Second pose translation, in world coordinates 24 | */ 25 | Mat computeHomography(const Mat& p, const Mat& n, const Mat& K, 26 | const Mat& R1, const Mat& t1, const Mat& R2, const Mat& t2) { 27 | 28 | Mat m = n.t() * (p - t1); 29 | Mat H = K * R2 * (m.at(0, 0) * Mat::eye(3, 3, CV_64FC1) + (t1 - t2) * n.t()) * R1.t() * K.inv(); 30 | 31 | return H; 32 | } 33 | 34 | /* 35 | * Returns a square region of interest given its center pixel coordinates and size. 36 | * Note that only if size is an odd number will the square be truly centered at the 37 | * desired position. 38 | * 39 | * center Center of the square 40 | * size Size of the square 41 | */ 42 | Rect buildSquare(const Point2i& center, int size) { 43 | 44 | int left = center.x - size / 2; 45 | int top = center.y - size / 2; 46 | 47 | return Rect(left, top, size, size); 48 | } 49 | 50 | /* 51 | * Removes the nth row and column of a square matrix. The matrix is not reduced 52 | * in-place, but a deep copy is returned. 53 | * 54 | * mat Matrix to be reduced 55 | * idx Row and column to be deleted 56 | */ 57 | Mat removeRowCol(const Mat& mat, int idx) { 58 | 59 | int n = mat.rows; 60 | 61 | Mat reduced(n - 1, n - 1, mat.type()); 62 | 63 | if (idx == 0) 64 | mat(Rect(1, 1, n - 1, n - 1)).copyTo(reduced(Rect(0, 0, n - 1, n - 1))); 65 | 66 | else if (idx == n - 1) 67 | mat(Rect(0, 0, n - 1, n - 1)).copyTo(reduced(Rect(0, 0, n - 1, n - 1))); 68 | 69 | else { 70 | mat(Rect(0, 0, idx, idx)).copyTo(reduced(Rect(0, 0, idx, idx))); 71 | mat(Rect(idx+1, 0, n-idx-1, idx)).copyTo(reduced(Rect(idx, 0, n-idx-1, idx))); 72 | mat(Rect(0, idx+1, idx, n-idx-1)).copyTo(reduced(Rect(0, idx, idx, n-idx-1))); 73 | mat(Rect(idx+1, idx+1, n-idx-1, n-idx-1)).copyTo(reduced(Rect(idx, idx, n-idx-1, n-idx-1))); 74 | } 75 | 76 | return reduced; 77 | } 78 | 79 | /* 80 | * Removes specific rows and columns of a square matrix. The matrix is not reduced in-place, 81 | * but a deep copy is returned. The indices to be removed should be sorted in ascending order. 82 | * The stride indicates how the matrix rows and columns are grouped together and referenced by 83 | * the indices. For example, given indices 0, 3, 7 and a stride of 2 the deleted rows and columns 84 | * are 0, 1, 6, 7, 14, 15. 85 | * 86 | * mat Matrix to be reduced 87 | * indices Sorted rows and columns to be deleted 88 | * stride Row and column stride 89 | */ 90 | Mat removeRowsCols(const Mat& mat, vector& indices, int stride) { 91 | 92 | Mat result = mat.clone(); 93 | 94 | for (unsigned int i = 0; i < indices.size(); i++) { 95 | 96 | int idx = stride * (indices[i] - i); 97 | 98 | for (int j = 0; j < stride; j++) 99 | result = removeRowCol(result, idx); 100 | } 101 | 102 | return result; 103 | } 104 | 105 | /* 106 | * Removes the nth row of a matrix. The matrix is not reduced in-place, but a deep 107 | * copy is returned. 108 | * 109 | * mat Matrix to be reduced 110 | * idx Row to be deleted 111 | */ 112 | Mat removeRow(const Mat& mat, int idx) { 113 | 114 | Mat result; 115 | 116 | int n = mat.rows; 117 | 118 | if (idx == 0) 119 | result = mat.rowRange(1, n).clone(); 120 | 121 | else if (idx == n - 1) 122 | result = mat.rowRange(0, n - 1).clone(); 123 | 124 | else 125 | vconcat(mat.rowRange(0, idx), mat.rowRange(idx + 1, n), result); 126 | 127 | return result; 128 | } 129 | 130 | /* 131 | * Removes specific rows of a matrix. The matrix is not reduced in-place, but a deep 132 | * copy is returned. The indices to be removed should be sorted in ascending order. 133 | * The stride indicates how the matrix rows are grouped together and referenced by the 134 | * indices. For example, given indices 0, 3, 7 and a stride of 2 the deleted rows are 135 | * 0, 1, 6, 7, 14, 15. 136 | * 137 | * mat Matrix to be reduced 138 | * indices Sorted rows to be deleted 139 | * stride Row stride 140 | */ 141 | Mat removeRows(const Mat& mat, vector& indices, int stride) { 142 | 143 | Mat result = mat.clone(); 144 | 145 | for (unsigned int i = 0; i < indices.size(); i++) { 146 | 147 | int idx = stride * (indices[i] - i); 148 | 149 | for (int j = 0; j < stride; j++) 150 | result = removeRow(result, idx); 151 | } 152 | 153 | return result; 154 | } 155 | 156 | /* 157 | * Returns the ellipses where the predicted in sight features should be found with 158 | * high probability. In particular, the ellipses are constructed so that they are 159 | * confidence regions at level 0.86. 160 | * 161 | * means Predicted in sight features pixel locations 162 | * S Innovation covariance matrix of the predicted in sight features 163 | */ 164 | vector computeEllipses(const vector& means, const Mat& S) { 165 | 166 | vector ellipses; 167 | 168 | for (unsigned int i = 0; i < means.size(); i++) { 169 | 170 | Mat Si = S(Rect(2*i, 2*i, 2, 2)); 171 | 172 | // Compute the eigenvalues and eigenvectors 173 | Mat eigenval, eigenvec; 174 | eigen(Si, eigenval, eigenvec); 175 | 176 | // Compute the angle between the largest eigenvector and the x axis 177 | double angle = atan2(eigenvec.at(0, 1), eigenvec.at(0, 0)); 178 | 179 | // Shift the angle from [-pi, pi] to [0, 2pi] 180 | if (angle < 0) 181 | angle += PI_DOUBLE; 182 | 183 | // Convert to degrees 184 | angle *= RAD_TO_DEG; 185 | 186 | // Compute the size of the major and minor axes 187 | double majorAxis = 4 * sqrt(eigenval.at(0)); 188 | double minorAxis = 4 * sqrt(eigenval.at(1)); 189 | 190 | ellipses.push_back(RotatedRect(means[i], Size(majorAxis, minorAxis), -angle)); 191 | } 192 | 193 | return ellipses; 194 | } 195 | 196 | /* 197 | * Returns the smallest straight rectangle which contains the given ellipse. The computed 198 | * rectangle is cropped if it exceeds the given image boundary so that it fits inside it. 199 | * 200 | * ellipse Ellipse to be bounded 201 | * imageSize Size of the image 202 | */ 203 | Rect getBoundingBox(const RotatedRect& ellipse, const Size& imageSize) { 204 | 205 | double angle = ellipse.angle * DEG_TO_RAD; 206 | double x = ellipse.center.x; 207 | double y = ellipse.center.y; 208 | double a = 0.5 * ellipse.size.width; 209 | double b = 0.5 * ellipse.size.height; 210 | 211 | double xmax, ymax; 212 | 213 | if (angle == 0 || a == b) { 214 | 215 | xmax = a; 216 | ymax = b; 217 | 218 | } else { 219 | 220 | double a2inv = 1 / (a*a); 221 | double b2inv = 1 / (b*b); 222 | 223 | double c = cos(angle); 224 | double s = sin(angle); 225 | double c2 = c*c; 226 | double s2 = s*s; 227 | 228 | double A = c2 * a2inv + s2 * b2inv; 229 | double B = c2 * b2inv + s2 * a2inv; 230 | double C = 2 * c * s * (b2inv - a2inv); 231 | 232 | double r = (4 * A * B / (C * C) - 1); 233 | double x0 = 1 / sqrt(A * r); 234 | double y0 = 1 / sqrt(B * r); 235 | 236 | if (C < 0) { 237 | 238 | x0 = - x0; 239 | y0 = - y0; 240 | } 241 | 242 | xmax = 2 * B * y0 / C; 243 | ymax = 2 * A * x0 / C; 244 | } 245 | 246 | double x0 = x - xmax; 247 | double y0 = y - ymax; 248 | double w = 2 * xmax + 1; 249 | double h = 2 * ymax + 1; 250 | 251 | if (x0 + w > imageSize.width) 252 | w = imageSize.width - x0; 253 | if (y0 + h > imageSize.height) 254 | h = imageSize.height - y0; 255 | 256 | return Rect(max(x - xmax, 0.), max(y - ymax, 0.), w, h); 257 | } 258 | 259 | /* 260 | * Draws a rectangle on the given image. The image is modified by the function so a deep 261 | * copy should be made in order to preserve the original. 262 | * 263 | * image Grayscale or color image to draw on 264 | * r Rectangle 265 | * color Color of the rectangle 266 | */ 267 | void drawRectangle(Mat& image, const Rect& r, const Scalar& color) { 268 | 269 | rectangle(image, r, color, 1, LINE_AA); 270 | } 271 | 272 | /* 273 | * Draws a square on the given image. The image is modified by the function so a deep 274 | * copy should be made in order to preserve the original. 275 | * 276 | * image Grayscale or color image to draw on 277 | * center Center of the square 278 | * width Width of the square 279 | * color Color of the square 280 | */ 281 | void drawSquare(Mat& image, const Point2i& center, int width, const Scalar& color) { 282 | 283 | rectangle(image, buildSquare(center, width), color, 1, LINE_AA); 284 | } 285 | 286 | /* 287 | * Draws an ellipse on the given image. The image is modified by the function so a deep 288 | * copy should be made in order to preserve the original. 289 | * 290 | * image Grayscale or color image to draw on 291 | * e Ellipse 292 | * color Color of the ellipse 293 | */ 294 | void drawEllipse(Mat& image, const RotatedRect& e, const Scalar& color) { 295 | 296 | ellipse(image, e, color, 1, LINE_AA); 297 | } 298 | 299 | /* 300 | * Draws a circle on the given image. The image is modified by the function so a deep 301 | * copy should be made in order to preserve the original. 302 | * 303 | * image Grayscale or color image to draw on 304 | * center Center of the circle 305 | * radius Radius of the circle 306 | * color Color of the circle 307 | */ 308 | void drawCircle(Mat& image, const Point2i& center, int radius, const Scalar& color) { 309 | 310 | circle(image, center, radius, color, 1, LINE_AA); 311 | } 312 | 313 | void drawTemplate(Mat& image, const Mat& templ, const Point2d& position, int cx, int cy) { 314 | 315 | int templ_x = 0; 316 | int image_x = position.x - cx; 317 | 318 | if (image_x > image.cols - 1) 319 | return; 320 | 321 | if (image_x < 0) { 322 | 323 | templ_x = - image_x; 324 | image_x = 0; 325 | } 326 | 327 | int templ_y = 0; 328 | int image_y = position.y - cy; 329 | 330 | if (image_y > image.rows - 1) 331 | return; 332 | 333 | if (image_y < 0) { 334 | 335 | templ_y = - image_y; 336 | image_y = 0; 337 | } 338 | 339 | int w = templ.cols - templ_x; 340 | 341 | if (w <= 0) 342 | return; 343 | 344 | if (image_x + w > image.cols) 345 | w = image.cols - image_x; 346 | 347 | int h = templ.rows - templ_y; 348 | 349 | if (h <= 0) 350 | return; 351 | 352 | if (image_y + h > image.rows) 353 | h = image.rows - image_y; 354 | 355 | templ(Rect(templ_x, templ_y, w, h)).copyTo(image(Rect(image_x, image_y, w, h))); 356 | } -------------------------------------------------------------------------------- /MonoSLAM/map.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | #include "opencv2/calib3d/calib3d.hpp" 5 | #include "opencv2/imgproc/imgproc.hpp" 6 | 7 | #include "map.hpp" 8 | #include "quaternion.hpp" 9 | #include "util.hpp" 10 | 11 | using namespace cv; 12 | using namespace std; 13 | 14 | /* 15 | * Initializes the map by detecting a known chessboard pattern. It provides an 16 | * absolute scale reference, some manually initialized features to track, and 17 | * the world frame origin and initial camera pose. The world frame origin is 18 | * positioned at the top left corner of the pattern, with the x axis pointing 19 | * along the pattern height and the y axis pointing along the pattern width. 20 | * Note that the z axis is determined by the right-hand rule (see solvePnP). 21 | * It is assumed the camera has zero linear and angular velocities. 22 | * 23 | * frame Grayscale frame 24 | * patternSize Pattern dimensions (see findChessboardCorners) 25 | * squareSize Size of a chessboard square, in meters 26 | * var Variances of the initial camera state components 27 | */ 28 | bool Map::initMap(const Mat& frame, const Size& patternSize, double squareSize, 29 | const vector& var) { 30 | 31 | int w = patternSize.width; 32 | int h = patternSize.height; 33 | 34 | vector imageCorners; 35 | 36 | bool patternFound = findChessboardCorners(frame, patternSize, imageCorners, 37 | CALIB_CB_ADAPTIVE_THRESH + 38 | CALIB_CB_NORMALIZE_IMAGE + 39 | CALIB_CB_FAST_CHECK); 40 | 41 | if (!patternFound) 42 | return false; 43 | 44 | cornerSubPix(frame, imageCorners, Size(11, 11), Size(-1, -1), 45 | TermCriteria(CV_TERMCRIT_EPS + CV_TERMCRIT_ITER, 30, 0.1)); 46 | 47 | vector worldCorners; 48 | 49 | // Set the world reference frame (y axis along pattern width) 50 | for (int i = 0; i < h; i++) 51 | for (int j = 0; j < w; j++) 52 | worldCorners.push_back(Point3d(squareSize * i, squareSize * j, 0)); 53 | 54 | Mat rvec, tvec; 55 | 56 | // Find camera pose 57 | bool poseFound = solvePnP(worldCorners, imageCorners, camera.K, camera.distCoeffs, rvec, tvec, 58 | false, SOLVEPNP_ITERATIVE); 59 | 60 | if (!poseFound) 61 | return false; 62 | 63 | Mat R, t; 64 | 65 | // Convert to rotation R (from world to camera) and translation t (in world coordinates) 66 | Rodrigues(rvec, R); 67 | t = - R.t() * tvec; 68 | 69 | // Compute the camera orientation quaternion 70 | Mat q = quaternionInv(computeQuaternion(rvec)); 71 | 72 | // Update the map state vector with the camera state vector (zero linear and angular velocities) 73 | t.copyTo(x(Rect(0, 0, 1, 3))); 74 | q.copyTo(x(Rect(0, 3, 1, 4))); 75 | 76 | // Update the map covariance matrix with the camera covariance matrix 77 | P = Mat::diag(Mat(var)); 78 | 79 | // Extract the chessboard outer and central corners and add the initial features 80 | int idx; 81 | bool init = true; 82 | 83 | idx = 0; 84 | init = init && addInitialFeature(frame, imageCorners[idx], worldCorners[idx], R, t); 85 | 86 | idx = w - 1; 87 | init = init && addInitialFeature(frame, imageCorners[idx], worldCorners[idx], R, t); 88 | 89 | idx = (h - 1) * w; 90 | init = init && addInitialFeature(frame, imageCorners[idx], worldCorners[idx], R, t); 91 | 92 | idx = w * h - 1; 93 | init = init && addInitialFeature(frame, imageCorners[idx], worldCorners[idx], R, t); 94 | 95 | idx = w * ((h - 1) / 2) + (w + 1) / 2 - 1; 96 | init = init && addInitialFeature(frame, imageCorners[idx], worldCorners[idx], R, t); 97 | 98 | if (!init) { 99 | 100 | reset(); 101 | return false; 102 | } 103 | 104 | return true; 105 | } 106 | 107 | /* 108 | * Updates the camera and features data with the map state vector and map state 109 | * covariance matrix data. Only shallow copies are performed. This function should 110 | * be called whenever the current state of the system must be broadcast to each 111 | * of its parts. This method exists because the map state (x and P) is continuously 112 | * updated by the EKF and these changes will not be directly reflected in the camera 113 | * and features instances. 114 | */ 115 | void Map::broadcastData() { 116 | 117 | camera.r = x(Rect(0, 0, 1, 3)); 118 | camera.q = x(Rect(0, 3, 1, 4)); 119 | camera.v = x(Rect(0, 7, 1, 3)); 120 | camera.w = x(Rect(0, 10, 1, 3)); 121 | 122 | camera.R = getRotationMatrix(camera.q); 123 | 124 | camera.P = P(Rect(0, 0, 13, 13)); 125 | 126 | for (unsigned int i = 0; i < features.size(); i++) { 127 | 128 | features[i].pos = x(Rect(0, 13 + 3*i, 1, 3)); 129 | features[i].P = P(Rect(13 + 3*i, 13 + 3*i, 3, 3)); 130 | } 131 | } 132 | 133 | /* 134 | * Detects corners and initializes new feature candidates whenever there are not 135 | * enough visible features and no candidates are being tracked. Returns a boolean 136 | * indicating whether new candidates were detected or not. 137 | * 138 | * frame Grayscale frame 139 | */ 140 | bool Map::trackNewCandidates(const Mat& frame) { 141 | 142 | // If there are enough visible features there is no need to detect new ones 143 | if (numVisibleFeatures >= minFeatureDensity) 144 | return false; 145 | 146 | // The same applies if there are few features but candidates are already being tracked 147 | if (!candidates.empty()) 148 | return false; 149 | 150 | Mat corners = findCorners(frame); 151 | 152 | // If no good corners were found just pass 153 | if (corners.empty()) 154 | return false; 155 | 156 | // Undistort and normalize the pixels 157 | Mat undistorted; 158 | undistortPoints(corners, undistorted, camera.K, camera.distCoeffs); 159 | 160 | // Add the z component and reshape to 3xN 161 | undistorted = undistorted.reshape(1).t(); 162 | undistorted.resize(3, Scalar(1)); 163 | 164 | // Get camera translation and rotation 165 | Mat t = x(Rect(0, 0, 1, 3)); 166 | Mat R = getRotationMatrix(x(Rect(0, 3, 1, 4))); 167 | 168 | // Compute the directions (in world coordinates) of the lines that contain the features 169 | undistorted.convertTo(undistorted, CV_64FC1); 170 | Mat directions = R * undistorted; 171 | 172 | Point2d depthInterval(0.05, 5); 173 | int depthSamples = 100; 174 | 175 | // Normalize the direction vectors and add new pre-initialized features 176 | for (int i = 0; i < directions.cols; i++) { 177 | 178 | normalize(directions.col(i), directions.col(i)); 179 | 180 | Point2i pixel(corners.at(i)); 181 | candidates.push_back(Feature(frame, buildSquare(pixel, patchSize), R.t(), t, 182 | directions.col(i), depthInterval, depthSamples)); 183 | } 184 | 185 | return true; 186 | } 187 | 188 | /* 189 | * Applies the first step of the Extended Kalman Filter. In particular, given the 190 | * current state estimate x_k|k and the current state covariance matrix P_k|k, it 191 | * computes the next a priori state estimate x_k+1|k (via the camera motion model) 192 | * and its associated covariance matrix P_k+1|k. 193 | * 194 | * dt Time interval between the current and past frames 195 | */ 196 | void Map::predict(double dt) { 197 | 198 | Mat F = computeProcessMatrix(dt); 199 | Mat N = computeProcessNoiseMatrix(dt, F); 200 | 201 | P = F * P * F.t() + N; 202 | 203 | applyMotionModel(dt); 204 | } 205 | 206 | /* 207 | * Applies the update step of the Extended Kalman Filter. In particular, given the 208 | * predicted (a priori) state estimate x_k+1|k and the associated covariance matrix 209 | * P_k+1|k, it computes the corrected (a posteriori) estimate x_k+1|k+1 and its 210 | * covariance P_k+1|k+1 by measuring known, currently visible map features. 211 | * 212 | * gray Grayscale frame, used for feature matching 213 | * frame Color frame, used as a canvas to draw features on 214 | */ 215 | void Map::update(const Mat& gray, Mat& frame) { 216 | 217 | vector inviewIndices; 218 | 219 | Mat H = computeMeasurementMatrix(inviewIndices); 220 | 221 | // If there are no predicted in sight features skip the update step 222 | if (inviewIndices.empty()) { 223 | 224 | numVisibleFeatures = 0; 225 | return; 226 | } 227 | 228 | Mat N(H.rows, H.rows, CV_64FC1, Scalar(0)); 229 | 230 | for (int i = 0; i < N.rows / 2; i++) { 231 | 232 | N.at(2*i, 2*i) = R.at(0, 0); 233 | N.at(2*i + 1, 2*i + 1) = R.at(1, 0); 234 | } 235 | 236 | // Compute the innovation covariance matrix 237 | Mat S = H * P * H.t() + N; 238 | 239 | // Compute the error ellipses of the predicted in sight features pixel positions 240 | vector ellipses = computeEllipses(inviewPos, S); 241 | 242 | vector matchedInviewIndices; // Positions of inviewIndices that contain matched features indices 243 | vector failedInviewIndices; // Positions of inviewIndices that contain failed features indices 244 | 245 | vector failedIndices; // Subset of inviewIndices: not matched features indices 246 | 247 | vector residuals; 248 | 249 | Feature *featuresPtr = features.data(); 250 | 251 | // For each predicted in sight feature 252 | for (unsigned int i = 0; i < ellipses.size(); i++) { 253 | 254 | int idx = inviewIndices[i]; 255 | 256 | Feature *feature = &featuresPtr[idx]; 257 | 258 | feature->matchingAttempts++; 259 | 260 | Mat p = x(Rect(0, 13 + 3*idx, 1, 3)); 261 | Mat n = feature->normal; 262 | Mat view1 = feature->image; 263 | Rect patch1 = feature->roi; 264 | Mat R1 = feature->R; 265 | Mat t1 = feature->t; 266 | Mat R2 = getRotationMatrix(x(Rect(0, 3, 1, 4))).t(); 267 | Mat t2 = x(Rect(0, 0, 1, 3)); 268 | 269 | // Compute its appearance from the current camera pose 270 | int u, v; 271 | Mat templ = camera.warpPatch(p, n, view1, patch1, R1, t1, R2, t2, u, v); 272 | 273 | // Show the warped template at the predicted in sight feature position 274 | Mat colorTempl; 275 | cvtColor(templ, colorTempl, CV_GRAY2RGB); 276 | drawTemplate(frame, colorTempl, inviewPos[i], u, v); 277 | 278 | // If the template size is zero the feature is far away and not visible 279 | if (templ.empty()) { 280 | 281 | feature->matchingFails++; 282 | failedInviewIndices.push_back(i); 283 | failedIndices.push_back(idx); 284 | 285 | } else { 286 | 287 | // Compute the rectangular region to match this template 288 | Rect roi = getBoundingBox(ellipses[i], gray.size()); 289 | 290 | int x0 = max(roi.x - u, 0); 291 | int y0 = max(roi.y - v, 0); 292 | int x1 = min(roi.x + roi.width + templ.cols - u, gray.cols - 1); 293 | int y1 = min(roi.y + roi.height + templ.rows - v, gray.rows - 1); 294 | 295 | roi.x = x0; 296 | roi.y = y0; 297 | roi.width = x1 - x0 + 1; 298 | roi.height = y1 - y0 + 1; 299 | 300 | Mat image = gray(roi); 301 | 302 | // Match the template 303 | Mat ccorr; 304 | matchTemplate(image, templ, ccorr, CV_TM_CCORR_NORMED); 305 | 306 | // Get the observation value 307 | double maxVal; 308 | Point2i maxLoc; 309 | minMaxLoc(ccorr, NULL, &maxVal, NULL, &maxLoc); 310 | 311 | if (maxVal > 0.5) { 312 | 313 | int px = maxLoc.x + roi.x + u; 314 | int py = maxLoc.y + roi.y + v; 315 | 316 | residuals.push_back(Point2d(px - inviewPos[i].x, py - inviewPos[i].y)); 317 | 318 | matchedInviewIndices.push_back(i); 319 | 320 | } else { 321 | 322 | feature->matchingFails++; 323 | failedInviewIndices.push_back(i); 324 | failedIndices.push_back(idx); 325 | } 326 | } 327 | } 328 | 329 | // Update the number of visible features 330 | numVisibleFeatures = matchedInviewIndices.size(); 331 | 332 | if (numVisibleFeatures == 0) { 333 | 334 | drawFeatures(frame, matchedInviewIndices, failedInviewIndices, ellipses); 335 | removeBadFeatures(failedInviewIndices, failedIndices); 336 | return; 337 | } 338 | 339 | // Compute measurement residual 340 | Mat y = Mat(residuals).t(); 341 | y = y.reshape(1).t(); 342 | 343 | if (!failedInviewIndices.empty()) { 344 | 345 | // Reshape measurement matrix H and innovation covariance S 346 | H = removeRows(H, failedInviewIndices, 2); 347 | S = removeRowsCols(S, failedInviewIndices, 2); 348 | } 349 | 350 | // Compute Kalman gain 351 | Mat K = P * H.t() * S.inv(); 352 | 353 | // Update the map state and the map state covariance matrix 354 | x += K * y; 355 | P -= K * H * P; 356 | 357 | drawFeatures(frame, matchedInviewIndices, failedInviewIndices, ellipses); 358 | removeBadFeatures(failedInviewIndices, failedIndices); 359 | renormalizeQuaternion(); 360 | } 361 | 362 | /* 363 | * Adds an initial feature to the map. Its position is known with zero uncertainty 364 | * hence the associated covariance matrix is the zero matrix. Moreover, since the 365 | * feature patch lies on the chessboard pattern its normal vector is taken to be 366 | * the z axis unit vector. The feature is also set as visible. If the feature patch 367 | * exceeds the frame boundaries the feature is not initialized and false is returned. 368 | * 369 | * frame Grayscale frame 370 | * pos2D Feature position in the image, in pixels 371 | * pos3D Feature position, in world coordinates 372 | * R Camera rotation (from world to camera) 373 | * t Camera position, in world coordinates 374 | */ 375 | bool Map::addInitialFeature(const Mat& frame, const Point2f& pos2D, const Point3f& pos3D, 376 | const Mat& R, const Mat& t) { 377 | 378 | // Resize the state vector to accommodate the new feature position 379 | x.resize(x.rows + 3); 380 | 381 | // Copy the new feature position into the state vector 382 | double pos3D_[] = {pos3D.x, pos3D.y, pos3D.z}; 383 | Mat(3, 1, CV_64FC1, pos3D_).copyTo(x(Rect(0, x.rows - 3, 1, 3))); 384 | 385 | // Resize the state covariance matrix 386 | Mat tmp(P.rows + 3, P.cols + 3, CV_64FC1, Scalar(0)); 387 | P.copyTo(tmp(Rect(0, 0, P.cols, P.rows))); 388 | P = tmp; 389 | 390 | // Compute the feature patch normal 391 | double n[] = {0, 0, 1}; 392 | Mat normal(3, 1, CV_64FC1, n); 393 | 394 | Point2i pixelPosition(pos2D); 395 | Rect roi = buildSquare(pixelPosition, patchSize); 396 | 397 | bool overflow = roi.x < 0 || roi.x + roi.width > frame.cols || 398 | roi.y < 0 || roi.y + roi.height > frame.rows; 399 | 400 | // Check if the feature patch is outside the frame boundaries 401 | if (overflow) 402 | return false; 403 | 404 | // Add the feature 405 | features.push_back(Feature(frame, roi, normal, R, t)); 406 | 407 | // Add its image position to the list of in view features 408 | inviewPos.push_back(pos2D); 409 | 410 | numVisibleFeatures++; 411 | 412 | return true; 413 | } 414 | 415 | /* 416 | * Sets the map as it was upon creation. 417 | */ 418 | void Map::reset() { 419 | 420 | features.clear(); 421 | candidates.clear(); 422 | inviewPos.clear(); 423 | 424 | numVisibleFeatures = 0; 425 | 426 | x = Mat(13, 1, CV_64FC1, Scalar(0)); 427 | P = Mat(13, 13, CV_64FC1, Scalar(0)); 428 | } 429 | 430 | /* 431 | * Returns corners found by the Shi-Tomasi corner detector. In particular, the 432 | * number of computed corners is such that when the associated pre-initialized 433 | * features are added to the map for tracking, the number of visible features 434 | * in the current frame will (hopefully) be the maximum allowed. 435 | * 436 | * frame Grayscale frame 437 | */ 438 | Mat Map::findCorners(const Mat& frame) { 439 | 440 | Mat corners; 441 | 442 | int maxCorners = maxFeatureDensity - numVisibleFeatures; 443 | int minDistance = 60; 444 | double qualityLevel = 0.2; 445 | 446 | Mat mask(frame.size(), CV_8UC1, Scalar(0)); 447 | 448 | // Set a margin around the mask to enclose the detected corners inside 449 | // a rectangle and avoid premature corner loss due to camera movement. 450 | // Pad should be larger than patchSize/2 so that new features patches 451 | // fit inside the camera frame. 452 | int pad = 30; 453 | mask(Rect(pad, pad, mask.cols - 2 * pad, mask.rows - 2 * pad)).setTo(Scalar(255)); 454 | 455 | for (unsigned int i = 0; i < inviewPos.size(); i++) { 456 | 457 | Rect roi = buildSquare(inviewPos[i], 2 * minDistance); 458 | 459 | roi.x = max(0, roi.x); 460 | roi.y = max(0, roi.y); 461 | 462 | if (roi.x + roi.width > frame.cols) 463 | roi.width = frame.cols - roi.x; 464 | if (roi.y + roi.height > frame.rows) 465 | roi.height = frame.rows - roi.y; 466 | 467 | // Update the mask to reject the region around the ith inview feature 468 | mask(roi).setTo(Scalar(0)); 469 | } 470 | 471 | goodFeaturesToTrack(frame, corners, maxCorners, qualityLevel, minDistance, mask); 472 | 473 | return corners; 474 | } 475 | 476 | /* 477 | * Applies the state transition function to the current state estimate. It is mandatory 478 | * that this method be called after computeProcessMatrix and computeProcessNoiseMatrix 479 | * since the latter expect the last a posteriori (corrected) state estimate in the state 480 | * vector x. 481 | * 482 | * dt Time interval between the current and past frames 483 | */ 484 | void Map::applyMotionModel(double dt) { 485 | 486 | x(Rect(0, 0, 1, 3)) += x(Rect(0, 7, 1, 3)) * dt; 487 | 488 | if (x.at(10, 0) != 0 || x.at(11, 0) != 0 || x.at(12, 0) != 0) 489 | x(Rect(0, 3, 1, 4)) = quaternionMultiply( 490 | x(Rect(0, 3, 1, 4)), 491 | computeQuaternion(x(Rect(0, 10, 1, 3)) * dt) 492 | ); 493 | } 494 | 495 | /* 496 | * Returns the Jacobian matrix of the state transition function with respect to the 497 | * complete state (r, q, v, w, f1, ..., fn). This matrix is evaluated at the current 498 | * state estimate on the assumption that there is zero process noise. This method must 499 | * be called before applyMotionModel since the latter updates the current estimate x. 500 | * 501 | * dt Time interval between the current and past frames 502 | */ 503 | Mat Map::computeProcessMatrix(double dt) { 504 | 505 | Mat F = Mat::eye(P.size(), CV_64FC1); 506 | 507 | // Set D(r_new)/D(v_old) 508 | 509 | F.at(0, 7) = dt; 510 | F.at(1, 8) = dt; 511 | F.at(2, 9) = dt; 512 | 513 | // Compute D(q_new/q(w*dt)) 514 | 515 | Mat Q1(4, 4, CV_64FC1); 516 | 517 | // Current pose quaternion (q_old) 518 | double a1 = x.at(3, 0); 519 | double b1 = x.at(4, 0); 520 | double c1 = x.at(5, 0); 521 | double d1 = x.at(6, 0); 522 | 523 | // Row 1 524 | Q1.at(0, 0) = a1; 525 | Q1.at(0, 1) = -b1; 526 | Q1.at(0, 2) = -c1; 527 | Q1.at(0, 3) = -d1; 528 | 529 | // Row 2 530 | Q1.at(1, 0) = b1; 531 | Q1.at(1, 1) = a1; 532 | Q1.at(1, 2) = -d1; 533 | Q1.at(1, 3) = c1; 534 | 535 | // Row 3 536 | Q1.at(2, 0) = c1; 537 | Q1.at(2, 1) = d1; 538 | Q1.at(2, 2) = a1; 539 | Q1.at(2, 3) = -b1; 540 | 541 | // Row 4 542 | Q1.at(3, 0) = d1; 543 | Q1.at(3, 1) = -c1; 544 | Q1.at(3, 2) = b1; 545 | Q1.at(3, 3) = a1; 546 | 547 | Mat Q2(4, 3, CV_64FC1); 548 | 549 | double w1 = x.at(10, 0); 550 | double w2 = x.at(11, 0); 551 | double w3 = x.at(12, 0); 552 | 553 | if (w1 == 0 && w2 == 0 && w3 == 0) { 554 | 555 | // Compute D(q(w*dt))/D(w_old) 556 | 557 | Q2.setTo(Scalar(0)); 558 | 559 | Q2.at(1, 0) = 0.5 * dt; 560 | Q2.at(2, 1) = 0.5 * dt; 561 | Q2.at(3, 2) = 0.5 * dt; 562 | 563 | } else { 564 | 565 | // Set D(q_new)/D(q_old) 566 | 567 | // Rotation quaternion q(w*dt) 568 | Mat q = computeQuaternion(x(Rect(0, 10, 1, 3)) * dt); 569 | 570 | double a2 = q.at(0, 0); 571 | double b2 = q.at(1, 0); 572 | double c2 = q.at(2, 0); 573 | double d2 = q.at(3, 0); 574 | 575 | // Row 1 576 | F.at(3, 3) = a2; 577 | F.at(3, 4) = -b2; 578 | F.at(3, 5) = -c2; 579 | F.at(3, 6) = -d2; 580 | 581 | // Row 2 582 | F.at(4, 3) = b2; 583 | F.at(4, 4) = a2; 584 | F.at(4, 5) = d2; 585 | F.at(4, 6) = -c2; 586 | 587 | // Row 3 588 | F.at(5, 3) = c2; 589 | F.at(5, 4) = -d2; 590 | F.at(5, 5) = a2; 591 | F.at(5, 6) = b2; 592 | 593 | // Row 4 594 | F.at(6, 3) = d2; 595 | F.at(6, 4) = c2; 596 | F.at(6, 5) = -b2; 597 | F.at(6, 6) = a2; 598 | 599 | // Compute D(q(w*dt))/D(w_old) 600 | 601 | double w11 = w1 * w1; 602 | double w22 = w2 * w2; 603 | double w33 = w3 * w3; 604 | 605 | double norm2 = w11 + w22 + w33; 606 | double norm = sqrt(norm2); 607 | 608 | double c = cos(0.5 * dt * norm); 609 | double s = sin(0.5 * dt * norm); 610 | 611 | double z = s / norm; 612 | 613 | double dq1dw_ = - 0.5 * dt * z; 614 | double dq_dw_ = 0.5 * dt * c / norm2 - z / norm2; 615 | 616 | double w12 = w1 * w2; 617 | double w13 = w1 * w3; 618 | double w23 = w2 * w3; 619 | 620 | // Row 1 621 | Q2.at(0, 0) = dq1dw_ * w1; 622 | Q2.at(0, 1) = dq1dw_ * w2; 623 | Q2.at(0, 2) = dq1dw_ * w3; 624 | 625 | // Row 2 626 | Q2.at(1, 0) = dq_dw_ * w11 + z; 627 | Q2.at(1, 1) = dq_dw_ * w12; 628 | Q2.at(1, 2) = dq_dw_ * w13; 629 | 630 | // Row 3 631 | Q2.at(2, 0) = dq_dw_ * w12; 632 | Q2.at(2, 1) = dq_dw_ * w22 + z; 633 | Q2.at(2, 2) = dq_dw_ * w23; 634 | 635 | // Row 4 636 | Q2.at(3, 0) = dq_dw_ * w13; 637 | Q2.at(3, 1) = dq_dw_ * w23; 638 | Q2.at(3, 2) = dq_dw_ * w33 + z; 639 | } 640 | 641 | // Set D(q_new)/D(w_old) 642 | 643 | F(Rect(10, 3, 3, 4)) = Q1 * Q2; 644 | 645 | return F; 646 | } 647 | 648 | /* 649 | * Returns the product W * Q * W^t, where Q is the process noise covariance matrix 650 | * and W is the Jacobian matrix of the state transition function with respect to 651 | * the noise vector (V1, V2, V3, W1, W2, W3). The latter is evaluated at the current 652 | * state estimate on the assumption that there is zero process noise. This method must 653 | * be called before applyMotionModel since the latter updates the current estimate x. 654 | * 655 | * dt Time interval between the current and past frames 656 | * F Jacobian matrix of the state transition function with respect to x 657 | */ 658 | Mat Map::computeProcessNoiseMatrix(double dt, const Mat& F) { 659 | 660 | Mat W0(7, 6, CV_64FC1, Scalar(0)); 661 | 662 | // Set D(r_new)/D(V) 663 | 664 | W0.at(0, 0) = dt; 665 | W0.at(1, 1) = dt; 666 | W0.at(2, 2) = dt; 667 | 668 | // Set D(q_new)/D(W) 669 | 670 | F(Rect(10, 3, 3, 4)).copyTo(W0(Rect(3, 3, 3, 4))); 671 | 672 | // Compute W * Q * W^t 673 | 674 | Mat Q = A * dt * dt; 675 | Mat W0_Q = W0 * Q; 676 | Mat W0_Q_t = W0_Q.t(); 677 | Mat W0_Q_W0 = W0_Q * W0.t(); 678 | 679 | Mat N(P.size(), CV_64FC1, Scalar(0)); 680 | 681 | W0_Q_W0.copyTo(N(Rect(0, 0, 7, 7))); 682 | W0_Q.copyTo(N(Rect(7, 0, 6, 7))); 683 | W0_Q_t.copyTo(N(Rect(0, 7, 7, 6))); 684 | Q.copyTo(N(Rect(7, 7, 6, 6))); 685 | 686 | return N; 687 | } 688 | 689 | /* 690 | * Returns the Jacobian matrix of the features measurement function with respect 691 | * to the complete state (r, q, v, w, f1, ..., fn). This matrix is evaluated at 692 | * the predicted (a priori) state estimate, therefore it should only be called 693 | * after applyMotionModel since the latter updates the current estimate x. Also, 694 | * it updates the vector of predicted insight features pixel positions, inviewPos, 695 | * and populates a vector with the predicted in sight features indices. 696 | */ 697 | Mat Map::computeMeasurementMatrix(vector& inviewIndices) { 698 | 699 | inviewPos.clear(); 700 | 701 | // Get the predicted camera rotation (from world basis to camera basis) 702 | Mat R = getRotationMatrix(x(Rect(0, 3, 1, 4))).t(); 703 | 704 | // Get the predicted camera position (in world coordinates) 705 | Mat t = x(Rect(0, 0, 1, 3)); 706 | 707 | vector points3D_(features.size()); 708 | 709 | // Set a vector of features positions 710 | for (unsigned int i = 0; i < points3D_.size(); i++) 711 | points3D_[i] = Point3d(x(Rect(0, 13 + 3*i, 1, 3))); 712 | 713 | vector points2D_; 714 | 715 | // Project all the features positions to the current view 716 | camera.projectPoints(R, t, points3D_, points2D_); 717 | 718 | vector points3D; 719 | 720 | Rect box(Point2i(0, 0), camera.frameSize); 721 | 722 | for (unsigned int i = 0; i < points2D_.size(); i++) { 723 | 724 | if (box.contains(points2D_[i])) { 725 | 726 | inviewIndices.push_back(i); 727 | inviewPos.push_back(points2D_[i]); 728 | 729 | points3D.push_back(points3D_[i]); 730 | } 731 | } 732 | 733 | Mat H(2 * inviewIndices.size(), x.rows, CV_64FC1, Scalar(0)); 734 | 735 | Mat R1, R2, R3, R4; 736 | 737 | getRotationMatrixDerivatives(x(Rect(0, 3, 1, 4)), R1, R2, R3, R4); 738 | 739 | double fx = camera.K.at(0, 0); 740 | double fy = camera.K.at(1, 1); 741 | 742 | double k1 = camera.distCoeffs.at(0, 0); 743 | double k2 = camera.distCoeffs.at(0, 1); 744 | double p1 = camera.distCoeffs.at(0, 2); 745 | double p2 = camera.distCoeffs.at(0, 3); 746 | double k3 = camera.distCoeffs.at(0, 4); 747 | 748 | for (int i = 0; i < H.rows / 2; i++) { 749 | 750 | Mat pt = Mat(points3D[i]) - t; 751 | 752 | Mat pCam = R * pt; 753 | 754 | double xCam = pCam.at(0, 0); 755 | double yCam = pCam.at(1, 0); 756 | double zCam = pCam.at(2, 0); 757 | 758 | double zCam2 = zCam * zCam; 759 | 760 | Mat pBar1 = R1 * pt; 761 | Mat pBar2 = R2 * pt; 762 | Mat pBar3 = R3 * pt; 763 | Mat pBar4 = R4 * pt; 764 | 765 | // D(xn)/D(r): derivatives of normalized camera coordinate xn with respect to camera position 766 | double dxndr1 = (R.at(2, 0) * xCam - R.at(0, 0) * zCam) / zCam2; 767 | double dxndr2 = (R.at(2, 1) * xCam - R.at(0, 1) * zCam) / zCam2; 768 | double dxndr3 = (R.at(2, 2) * xCam - R.at(0, 2) * zCam) / zCam2; 769 | 770 | // D(xn)/D(q): derivatives of normalized camera coordinate xn with respect to camera orientation 771 | double dxndq1 = (pBar1.at(0, 0) * zCam - pBar1.at(2, 0) * xCam) / zCam2; 772 | double dxndq2 = (pBar2.at(0, 0) * zCam - pBar2.at(2, 0) * xCam) / zCam2; 773 | double dxndq3 = (pBar3.at(0, 0) * zCam - pBar3.at(2, 0) * xCam) / zCam2; 774 | double dxndq4 = (pBar4.at(0, 0) * zCam - pBar4.at(2, 0) * xCam) / zCam2; 775 | 776 | // D(yn)/D(r): derivatives of normalized camera coordinate yn with respect to camera position 777 | double dyndr1 = (R.at(2, 0) * yCam - R.at(1, 0) * zCam) / zCam2; 778 | double dyndr2 = (R.at(2, 1) * yCam - R.at(1, 1) * zCam) / zCam2; 779 | double dyndr3 = (R.at(2, 2) * yCam - R.at(1, 2) * zCam) / zCam2; 780 | 781 | // D(yn)/D(q): derivatives of normalized camera coordinate yn with respect to camera orientation 782 | double dyndq1 = (pBar1.at(1, 0) * zCam - pBar1.at(2, 0) * yCam) / zCam2; 783 | double dyndq2 = (pBar2.at(1, 0) * zCam - pBar2.at(2, 0) * yCam) / zCam2; 784 | double dyndq3 = (pBar3.at(1, 0) * zCam - pBar3.at(2, 0) * yCam) / zCam2; 785 | double dyndq4 = (pBar4.at(1, 0) * zCam - pBar4.at(2, 0) * yCam) / zCam2; 786 | 787 | double xn = xCam / zCam; 788 | double yn = yCam / zCam; 789 | 790 | // D(r2)/D(r): derivatives of r^2 = xn^2 + yn^2 with respect to camera position 791 | double dr2dr1 = 2 * (xn * dxndr1 + yn * dyndr1); 792 | double dr2dr2 = 2 * (xn * dxndr2 + yn * dyndr2); 793 | double dr2dr3 = 2 * (xn * dxndr3 + yn * dyndr3); 794 | 795 | // D(r2)/D(q): derivatives of r^2 = xn^2 + yn^2 with respect to camera orientation 796 | double dr2dq1 = 2 * (xn * dxndq1 + yn * dyndq1); 797 | double dr2dq2 = 2 * (xn * dxndq2 + yn * dyndq2); 798 | double dr2dq3 = 2 * (xn * dxndq3 + yn * dyndq3); 799 | double dr2dq4 = 2 * (xn * dxndq4 + yn * dyndq4); 800 | 801 | double r2 = xn * xn + yn * yn; 802 | double r4 = r2 * r2; 803 | double r6 = r4 * r2; 804 | 805 | double a = 1 + k1*r2 + k2*r4 + k3*r6; 806 | double b = k1 + 2*k2*r2 + 3*k3*r4; 807 | 808 | double ddr1 = 2 * (xn * dyndr1 + yn * dxndr1); 809 | double ddr2 = 2 * (xn * dyndr2 + yn * dxndr2); 810 | double ddr3 = 2 * (xn * dyndr3 + yn * dxndr3); 811 | 812 | double ddq1 = 2 * (xn * dyndq1 + yn * dxndq1); 813 | double ddq2 = 2 * (xn * dyndq2 + yn * dxndq2); 814 | double ddq3 = 2 * (xn * dyndq3 + yn * dxndq3); 815 | double ddq4 = 2 * (xn * dyndq4 + yn * dxndq4); 816 | 817 | // D(u)/D(r) 818 | double dudr1 = fx * (dxndr1*a + xn*dr2dr1*b + p1*ddr1 + p2*(dr2dr1 + 4*xn*dxndr1)); 819 | double dudr2 = fx * (dxndr2*a + xn*dr2dr2*b + p1*ddr2 + p2*(dr2dr2 + 4*xn*dxndr2)); 820 | double dudr3 = fx * (dxndr3*a + xn*dr2dr3*b + p1*ddr3 + p2*(dr2dr3 + 4*xn*dxndr3)); 821 | 822 | // D(u)/D(x) 823 | double dudx1 = - dudr1; 824 | double dudx2 = - dudr2; 825 | double dudx3 = - dudr3; 826 | 827 | // D(u)/D(q) 828 | double dudq1 = fx * (dxndq1*a + xn*dr2dq1*b + p1*ddq1 + p2*(dr2dq1 + 4*xn*dxndq1)); 829 | double dudq2 = fx * (dxndq2*a + xn*dr2dq2*b + p1*ddq2 + p2*(dr2dq2 + 4*xn*dxndq2)); 830 | double dudq3 = fx * (dxndq3*a + xn*dr2dq3*b + p1*ddq3 + p2*(dr2dq3 + 4*xn*dxndq3)); 831 | double dudq4 = fx * (dxndq4*a + xn*dr2dq4*b + p1*ddq4 + p2*(dr2dq4 + 4*xn*dxndq4)); 832 | 833 | // D(v)/D(r) 834 | double dvdr1 = fy * (dyndr1*a + yn*dr2dr1*b + p2*ddr1 + p1*(dr2dr1 + 4*yn*dyndr1)); 835 | double dvdr2 = fy * (dyndr2*a + yn*dr2dr2*b + p2*ddr2 + p1*(dr2dr2 + 4*yn*dyndr2)); 836 | double dvdr3 = fy * (dyndr3*a + yn*dr2dr3*b + p2*ddr3 + p1*(dr2dr3 + 4*yn*dyndr3)); 837 | 838 | // D(v)/D(x) 839 | double dvdx1 = - dvdr1; 840 | double dvdx2 = - dvdr2; 841 | double dvdx3 = - dvdr3; 842 | 843 | // D(v)/D(q) 844 | double dvdq1 = fy * (dyndq1*a + yn*dr2dq1*b + p2*ddq1 + p1*(dr2dq1 + 4*yn*dyndq1)); 845 | double dvdq2 = fy * (dyndq2*a + yn*dr2dq2*b + p2*ddq2 + p1*(dr2dq2 + 4*yn*dyndq2)); 846 | double dvdq3 = fy * (dyndq3*a + yn*dr2dq3*b + p2*ddq3 + p1*(dr2dq3 + 4*yn*dyndq3)); 847 | double dvdq4 = fy * (dyndq4*a + yn*dr2dq4*b + p2*ddq4 + p1*(dr2dq4 + 4*yn*dyndq4)); 848 | 849 | H.at(2*i, 0) = dudr1; 850 | H.at(2*i, 1) = dudr2; 851 | H.at(2*i, 2) = dudr3; 852 | H.at(2*i, 3) = dudq1; 853 | H.at(2*i, 4) = dudq2; 854 | H.at(2*i, 5) = dudq3; 855 | H.at(2*i, 6) = dudq4; 856 | 857 | H.at(2*i, 13 + 3*i) = dudx1; 858 | H.at(2*i, 13 + 3*i+1) = dudx2; 859 | H.at(2*i, 13 + 3*i+2) = dudx3; 860 | 861 | H.at(2*i+1, 0) = dvdr1; 862 | H.at(2*i+1, 1) = dvdr2; 863 | H.at(2*i+1, 2) = dvdr3; 864 | H.at(2*i+1, 3) = dvdq1; 865 | H.at(2*i+1, 4) = dvdq2; 866 | H.at(2*i+1, 5) = dvdq3; 867 | H.at(2*i+1, 6) = dvdq4; 868 | 869 | H.at(2*i+1, 13 + 3*i) = dvdx1; 870 | H.at(2*i+1, 13 + 3*i+1) = dvdx2; 871 | H.at(2*i+1, 13 + 3*i+2) = dvdx3; 872 | } 873 | 874 | return H; 875 | } 876 | 877 | void Map::drawFeatures(Mat& frame, 878 | const vector& matchedInviewIndices, const vector& failedInviewIndices, 879 | const vector& ellipses, bool drawEllipses) { 880 | 881 | if (drawEllipses) { 882 | 883 | // Draw matched features ellipses (red) 884 | for (unsigned int i = 0; i < matchedInviewIndices.size(); i++) { 885 | 886 | int idx = matchedInviewIndices[i]; 887 | 888 | drawEllipse(frame, ellipses[idx], Scalar(0, 0, 255)); 889 | } 890 | 891 | // Draw failed features ellipses (blue) 892 | for (unsigned int i = 0; i < failedInviewIndices.size(); i++) { 893 | 894 | int idx = failedInviewIndices[i]; 895 | 896 | drawEllipse(frame, ellipses[idx], Scalar(255, 0, 0)); 897 | } 898 | 899 | } else { 900 | 901 | // Draw matched features (red) 902 | for (unsigned int i = 0; i < matchedInviewIndices.size(); i++) { 903 | 904 | int idx = matchedInviewIndices[i]; 905 | 906 | drawSquare(frame, inviewPos[idx], 11, Scalar(0, 0, 255)); 907 | } 908 | } 909 | } 910 | 911 | void Map::removeBadFeatures(const vector& failedInviewIndices, const vector& failedIndices) { 912 | 913 | vector badFeaturesVecIndices; // Indices to remove from features, x and P 914 | vector badFeaturesPosIndices; // Indices to remove from inviewPos 915 | 916 | for (unsigned int i = 0; i < failedIndices.size(); i++) { 917 | 918 | int idx = failedIndices[i]; 919 | int j = failedInviewIndices[i]; 920 | 921 | double numFails = features[idx].matchingFails; 922 | double numAttemps = features[idx].matchingAttempts; 923 | 924 | if (numFails / numAttemps > failTolerance) { 925 | 926 | badFeaturesVecIndices.push_back(idx); 927 | badFeaturesPosIndices.push_back(j); 928 | } 929 | } 930 | 931 | removeIndices(features, badFeaturesVecIndices); 932 | removeIndices(inviewPos, badFeaturesPosIndices); 933 | 934 | Mat x_Features = x(Rect(0, 13, 1, x.rows - 13)); 935 | Mat P_Features = P(Rect(13, 13, x.rows - 13, x.rows - 13)); 936 | 937 | x_Features = removeRows(x_Features, badFeaturesVecIndices, 3); 938 | P_Features = removeRowsCols(P_Features, badFeaturesVecIndices, 3); 939 | } 940 | 941 | /* 942 | * Normalizes the system state orientation quaternion and updates the covariance 943 | * matrix accordingly by first-order error propagation. It is mandatory that the 944 | * function be called after the EKF update step since the corrected (a posteriori) 945 | * orientation quaternion might not be a unit quaternion. 946 | */ 947 | void Map::renormalizeQuaternion() { 948 | 949 | Mat q = x(Rect(0, 3, 1, 4)); 950 | 951 | double q1 = q.at(0, 0); 952 | double q2 = q.at(1, 0); 953 | double q3 = q.at(2, 0); 954 | double q4 = q.at(3, 0); 955 | 956 | double q11 = q1 * q1; 957 | double q22 = q2 * q2; 958 | double q33 = q3 * q3; 959 | double q44 = q4 * q4; 960 | 961 | double j1 = q22 + q33 + q44; 962 | double j2 = q11 + q33 + q44; 963 | double j3 = q11 + q22 + q44; 964 | double j4 = q11 + q22 + q33; 965 | 966 | double j12 = - q1 * q2; 967 | double j13 = - q1 * q3; 968 | double j14 = - q1 * q4; 969 | double j23 = - q2 * q3; 970 | double j24 = - q2 * q4; 971 | double j34 = - q3 * q4; 972 | 973 | Mat J = Mat::eye(P.size(), P.type()); 974 | 975 | J.at(3, 3) = j1; 976 | J.at(3, 4) = j12; 977 | J.at(3, 5) = j13; 978 | J.at(3, 6) = j14; 979 | 980 | J.at(4, 3) = j12; 981 | J.at(4, 4) = j2; 982 | J.at(4, 5) = j23; 983 | J.at(4, 6) = j24; 984 | 985 | J.at(5, 3) = j13; 986 | J.at(5, 4) = j23; 987 | J.at(5, 5) = j3; 988 | J.at(5, 6) = j34; 989 | 990 | J.at(6, 3) = j14; 991 | J.at(6, 4) = j24; 992 | J.at(6, 5) = j34; 993 | J.at(6, 6) = j4; 994 | 995 | double qnorm = norm(q); 996 | 997 | J(Rect(3, 3, 4, 4)) /= qnorm * qnorm * qnorm; 998 | 999 | // Normalize quaternion 1000 | normalize(q, q); 1001 | 1002 | // Update covariance matrix 1003 | P = J * P * J.t(); 1004 | } --------------------------------------------------------------------------------