docs: add link to raspberry article in glossary

* Add rus article about install ROS

* Add lines

* Add article about launching gazebo

* Some fix

* Add fix

* Fix

* Text fix

* Fix MD exceptions

* docs: edit sitl articles

* article fix

.markdownlint.json

@@ -24,6 +24,8 @@
             "Python",
             "C++",
             "PX4",
+            "px4.io",
+            "logs.px4.io",
             "QGroundControl",
             "QGC",
             "WireShark",
@@ -38,6 +40,10 @@
             "RPi",
             "Linux",
             "Windows",
+            "Docker",
+            "Travis",
+            "travis-ci.org",
+            "travis-ci.com",
             "macOS",
             "iOS",
             "Android",

.travis.yml

@@ -10,10 +10,11 @@ env:
     - IMAGE_NAME="$(basename -s '.git' ${TARGET_REPO})_${IMAGE_VERSION}.img"
 git:
   depth: 50
-matrix:
+jobs:
   fast_finish: true
   include:
-    - name: "Raspberry Pi Image Build"
+    - stage: Build
+      name: "Raspberry Pi Image Build"
       cache:
         directories:
         - imgcache
@@ -40,7 +41,8 @@ matrix:
           tags: true
         draft: true
         name: ${TRAVIS_TAG}
-    - name: "Documentation"
+    - stage: Build
+      name: "Documentation"
       language: node_js
       node_js:
         - "10"
@@ -53,20 +55,40 @@ matrix:
         - markdownlint docs
         - gitbook install
         - gitbook build
-# ***
+      deploy:
-# Disable deployments for now, revisit this later
+        provider: pages
-#   --sfalexrog, 06.02.2019
+        local-dir: _book
-# ***
+        skip-cleanup: true
-#      deploy:
+        github-token: ${GITHUB_OAUTH_TOKEN}
-#        provider: pages
+        keep-history: true
-#        local-dir: _book
+        target-branch: master
-#        skip-cleanup: true
+        repo: CopterExpress/clever-gitbook
-#        github-token: ${GITHUB_OAUTH_TOKEN}
+        fqdn: clever.copterexpress.com
-#        keep-history: true
+        verbose: true
-#        target-branch: gh-pages
+        on:
-#        on:
+          branch: master
-#          branch: WIP/gitbook-autobuild
+      deploy:
-
+        provider: pages
+        local-dir: _book
+        skip-cleanup: true
+        github-token: ${GITHUB_OAUTH_TOKEN}
+        keep-history: false
+        target-branch: master
+        repo: okalachev/cl4wip
+        verbose: true
+        on:
+          branch: clever4
+    - stage: Annotate
+      name: Auto-generate changelog
+      language: python
+      python: 3.6
+      install:
+        - pip install GitPython PyGithub
+      script:
+        - PYTHONUNBUFFERED=1 python ./gen_changelog.py
+stages:
+  - Build
+  - Annotate
 # More info there
 # https://github.com/travis-ci/travis-ci/issues/6893
 # https://docs.travis-ci.com/user/customizing-the-build/

apps/ios/README.md

@@ -1,5 +1,4 @@
-iOS-приложение для управления Клевером
+# iOS-приложение для управления Клевером
---------------------------------------
 
 Для установки зависимостей необходим [CocoaPods](https://cocoapods.org):
 
@@ -8,3 +7,11 @@ pod install
 ```
 
 Для разработки и сборки откройте в XCode файл `cleverrc.xcworkspace`.
+
+## Политика конфиденциальности
+
+App Store приложение CLEVER RC не собирает и не хранит каких-либо личных данных пользователя.
+
+## Privacy policy
+
+The App Store app CLEVER RC does not collect and store any personal user data.

aruco_pose/README.md

@@ -74,6 +74,7 @@ It's recommended to run it within the same nodelet manager with the camera nodel
 * `~image_width` – debug image width (default: 2000)
 * `~image_height` – debug image height (default: 2000)
 * `~image_margin` – debug image margin (default: 200)
+* `~dictionary` (*int*) – ArUco dictionary (default: 2) - should be the same as `dictionary` parameter of `aruco_detect` nodelet
 
 Map file has one marker per line with the following line format:
 

aruco_pose/map/office_ceiling.txt

@@ -1,8 +1,8 @@
-14  0.365 0.000  0.0 0 0 0 0
+14	0.365	0.000	0.0	0	0	0	0
-15  0.365 1.335  0.0 0 0 0 0
+15	0.365	1.335	0.0	0	0	0	0
-30  0.365 2.865  0.0 0 0 0 0
+30	0.365	2.865	0.0	0	0	0	0
-31  0.365 4.200  0.0 0 0 0 0
+31	0.365	4.200	0.0	0	0	0	0
-12  0.365 0.000  1.8 0 0 0 0
+12	0.365	0.000	1.8	0	0	0	0
 13	0.365	1.335	1.8	0	0	0	0
 28	0.365	2.865	1.8	0	0	0	0
 29	0.365	4.200	1.8	0	0	0	0
@@ -22,10 +22,10 @@
 5	0.365	1.335	9.0	0	0	0	0
 20	0.365	2.865	9.0	0	0	0	0
 21	0.365	4.200	9.0	0	0	0	0
-2	0.365	0.000	0.8	0	0	0	0
+2	0.365	0.000	10.8	0	0	0	0
-3	0.365	1.335	0.8	0	0	0	0
+3	0.365	1.335	10.8	0	0	0	0
-18	0.365	2.865	0.8	0	0	0	0
+18	0.365	2.865	10.8	0	0	0	0
-19	0.365	4.200	0.8	0	0	0	0
+19	0.365	4.200	10.8	0	0	0	0
-1	0.365	0.000	2.6	0	0	0	0
+1	0.365	0.000	12.6	0	0	0	0
-0	0.365	1.335	2.6	0	0	0	0
+0	0.365	1.335	12.6	0	0	0	0
-16	0.365	2.865	2.6	0	0	0	0
+16	0.365	2.865	12.6	0	0	0	0

aruco_pose/src/aruco_detect.cpp

@@ -62,7 +62,7 @@ private:
 	image_transport::Publisher debug_pub_;
 	image_transport::CameraSubscriber img_sub_;
 	ros::Publisher markers_pub_, vis_markers_pub_;
-	bool estimate_poses_, send_tf_;
+	bool estimate_poses_, send_tf_, auto_flip_;
 	double length_;
 	std::unordered_map<int, double> length_override_;
 	std::string frame_id_prefix_, known_tilt_;
@@ -87,6 +87,8 @@ public:
 		readLengthOverride();
 
 		nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
+		nh_priv_.param("auto_flip", auto_flip_, false);
+
 		nh_priv_.param<std::string>("frame_id_prefix", frame_id_prefix_, "aruco_");
 
 		camera_matrix_ = cv::Mat::zeros(3, 3, CV_64F);
@@ -177,7 +179,7 @@ private:
 
 					// snap orientation (if enabled and snap frame available)
 					if (!known_tilt_.empty() && !snap_to.header.frame_id.empty()) {
-						snapOrientation(marker.pose.orientation, snap_to.transform.rotation);
+						snapOrientation(marker.pose.orientation, snap_to.transform.rotation, auto_flip_);
 					}
 
 					// TODO: check IDs are unique

aruco_pose/src/aruco_map.cpp

@@ -73,6 +73,7 @@ private:
 	visualization_msgs::MarkerArray vis_array_;
 	std::string known_tilt_;
 	int image_width_, image_height_, image_margin_;
+	bool auto_flip_;
 
 public:
 	virtual void onInit()
@@ -95,6 +96,7 @@ public:
 		nh_priv_.param<std::string>("type", type, "map");
 		nh_priv_.param<std::string>("frame_id", transform_.child_frame_id, "aruco_map");
 		nh_priv_.param<std::string>("known_tilt", known_tilt_, "");
+		nh_priv_.param("auto_flip", auto_flip_, false);
 		nh_priv_.param("image_width", image_width_, 2000);
 		nh_priv_.param("image_height", image_height_, 2000);
 		nh_priv_.param("image_margin", image_margin_, 200);
@@ -183,7 +185,7 @@ public:
 			try {
 				geometry_msgs::TransformStamped snap_to = tf_buffer_.lookupTransform(markers->header.frame_id,
 				                                          known_tilt_, markers->header.stamp, ros::Duration(0.02));
-				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation);
+				snapOrientation(transform_.transform.rotation, snap_to.transform.rotation, auto_flip_);
 			} catch (const tf2::TransformException& e) {
 				ROS_WARN_THROTTLE(1, "aruco_map: can't snap: %s", e.what());
 			}
@@ -217,7 +219,7 @@ publish_debug:
 			Mat mat = cv_bridge::toCvCopy(image, "bgr8")->image; // copy image as we're planning to modify it
 			cv::aruco::drawDetectedMarkers(mat, corners, ids); // draw detected markers
 			if (valid) {
-				cv::aruco::drawAxis(mat, camera_matrix_, dist_coeffs_, rvec, tvec, 1.0); // draw board axis
+				_drawAxis(mat, camera_matrix_, dist_coeffs_, rvec, tvec, 1.0); // draw board axis
 			}
 			cv_bridge::CvImage out_msg;
 			out_msg.header.frame_id = image->header.frame_id;

aruco_pose/src/draw.cpp

@@ -6,6 +6,22 @@
 using namespace cv;
 using namespace cv::aruco;
 
+static void _cvProjectPoints2( const CvMat* object_points, const CvMat* rotation_vector,
+                       const CvMat* translation_vector, const CvMat* camera_matrix,
+                       const CvMat* distortion_coeffs, CvMat* image_points,
+                       CvMat* dpdrot CV_DEFAULT(NULL), CvMat* dpdt CV_DEFAULT(NULL),
+                       CvMat* dpdf CV_DEFAULT(NULL), CvMat* dpdc CV_DEFAULT(NULL),
+                       CvMat* dpddist CV_DEFAULT(NULL),
+                       double aspect_ratio CV_DEFAULT(0));
+
+static void _projectPoints( InputArray objectPoints,
+                    InputArray rvec, InputArray tvec,
+                    InputArray cameraMatrix, InputArray distCoeffs,
+                    OutputArray imagePoints,
+                    OutputArray jacobian = noArray(),
+                    double aspectRatio = 0 );
+
+
 void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginSize,
                       int borderBits) {
 
@@ -85,3 +101,697 @@ void _drawPlanarBoard(Board *_board, Size outSize, OutputArray _img, int marginS
 						BORDER_TRANSPARENT);
 	}
 }
+
+/* Draw a (potentially partially visible) line. */
+static void linePartial(InputOutputArray image, Point3f p1, Point3f p2, const Scalar& color,
+        int thickness = 1, int lineType = LINE_8, int shift = 0)
+{
+    // If both points are behind the screen, don't draw anything
+    if (p1.z <= 0 && p2.z <= 0)
+    {
+        return;
+    }
+    Point2f p1p{p1.x, p1.y};
+    Point2f p2p{p2.x, p2.y};
+    // If points are on the different sides of the plane, compute intersection point
+    if (p1.z * p2.z < 0)
+    {
+        // Compute intersection point with the screen
+        // We denote alpha as such:
+        // xi = (1 - alpha) * x1 + alpha * x2
+        // yi = (1 - alpha) * y1 + alpha * y2
+        // zi = (1 - alpha) * z1 + alpha * z2 = 0
+        // Thus, alpha can be expressed as
+        // alpha = z1 / (z1 - z2)
+        float alpha = p1.z / (p1.z - p2.z);
+        Point2f pi{(1 - alpha) * p1.x + alpha * p2.x, (1 - alpha) * p1.y + alpha * p2.y};
+        // Now, if z1 is negative, we draw the line from (xi, yi) to (x2, y2), else we draw from (x1, y1) to (xi, yi)
+        if (p1.z < 0)
+        {
+            p1p = pi;
+        }
+        else
+        {
+            p2p = pi;
+        }
+    }
+    line(image, p1p, p2p, color, thickness, lineType, shift);
+}
+
+void _drawAxis(InputOutputArray _image, InputArray _cameraMatrix, InputArray _distCoeffs,
+              InputArray _rvec, InputArray _tvec, float length) {
+
+    CV_Assert(_image.getMat().total() != 0 &&
+              (_image.getMat().channels() == 1 || _image.getMat().channels() == 3));
+    CV_Assert(length > 0);
+
+    // project axis points
+    std::vector< Point3f > axisPoints;
+    axisPoints.push_back(Point3f(0, 0, 0));
+    axisPoints.push_back(Point3f(length, 0, 0));
+    axisPoints.push_back(Point3f(0, length, 0));
+    axisPoints.push_back(Point3f(0, 0, length));
+    std::vector< Point3f > imagePointsZ;
+    _projectPoints(axisPoints, _rvec, _tvec, _cameraMatrix, _distCoeffs, imagePointsZ);
+
+    // draw axis lines
+    linePartial(_image, imagePointsZ[0], imagePointsZ[1], Scalar(0, 0, 255), 3);
+    linePartial(_image, imagePointsZ[0], imagePointsZ[2], Scalar(0, 255, 0), 3);
+    linePartial(_image, imagePointsZ[0], imagePointsZ[3], Scalar(255, 0, 0), 3);
+}
+
+static CvMat _cvMat(const cv::Mat& m)
+{
+    CvMat self;
+    CV_DbgAssert(m.dims <= 2);
+    self = cvMat(m.rows, m.dims == 1 ? 1 : m.cols, m.type(), m.data);
+    self.step = (int)m.step[0];
+    self.type = (self.type & ~cv::Mat::CONTINUOUS_FLAG) | (m.flags & cv::Mat::CONTINUOUS_FLAG);
+    return self;
+}
+
+static void _projectPoints( InputArray _opoints,
+                        InputArray _rvec,
+                        InputArray _tvec,
+                        InputArray _cameraMatrix,
+                        InputArray _distCoeffs,
+                        OutputArray _ipoints,
+                        OutputArray _jacobian,
+                        double aspectRatio )
+{
+    Mat opoints = _opoints.getMat();
+    int npoints = opoints.checkVector(3), depth = opoints.depth();
+    CV_Assert(npoints >= 0 && (depth == CV_32F || depth == CV_64F));
+
+    CvMat dpdrot, dpdt, dpdf, dpdc, dpddist;
+    CvMat *pdpdrot=0, *pdpdt=0, *pdpdf=0, *pdpdc=0, *pdpddist=0;
+
+    CV_Assert( _ipoints.needed() );
+
+    _ipoints.create(npoints, 1, CV_MAKETYPE(depth, 3), -1, true);
+    Mat imagePoints = _ipoints.getMat();
+    CvMat c_imagePoints = _cvMat(imagePoints);
+    CvMat c_objectPoints = _cvMat(opoints);
+    Mat cameraMatrix = _cameraMatrix.getMat();
+
+    Mat rvec = _rvec.getMat(), tvec = _tvec.getMat();
+    CvMat c_cameraMatrix = _cvMat(cameraMatrix);
+    CvMat c_rvec = _cvMat(rvec), c_tvec = _cvMat(tvec);
+
+    double dc0buf[5]={0};
+    Mat dc0(5,1,CV_64F,dc0buf);
+    Mat distCoeffs = _distCoeffs.getMat();
+    if( distCoeffs.empty() )
+        distCoeffs = dc0;
+    CvMat c_distCoeffs = _cvMat(distCoeffs);
+    int ndistCoeffs = distCoeffs.rows + distCoeffs.cols - 1;
+
+    Mat jacobian;
+    if( _jacobian.needed() )
+    {
+        _jacobian.create(npoints*2, 3+3+2+2+ndistCoeffs, CV_64F);
+        jacobian = _jacobian.getMat();
+        pdpdrot = &(dpdrot = _cvMat(jacobian.colRange(0, 3)));
+        pdpdt = &(dpdt = _cvMat(jacobian.colRange(3, 6)));
+        pdpdf = &(dpdf = _cvMat(jacobian.colRange(6, 8)));
+        pdpdc = &(dpdc = _cvMat(jacobian.colRange(8, 10)));
+        pdpddist = &(dpddist = _cvMat(jacobian.colRange(10, 10+ndistCoeffs)));
+    }
+
+    _cvProjectPoints2( &c_objectPoints, &c_rvec, &c_tvec, &c_cameraMatrix, &c_distCoeffs,
+                      &c_imagePoints, pdpdrot, pdpdt, pdpdf, pdpdc, pdpddist, aspectRatio );
+}
+
+namespace _detail
+{
+    template <typename FLOAT>
+    void computeTiltProjectionMatrix(FLOAT tauX,
+                                     FLOAT tauY,
+                                     Matx<FLOAT, 3, 3>* matTilt = 0,
+                                     Matx<FLOAT, 3, 3>* dMatTiltdTauX = 0,
+                                     Matx<FLOAT, 3, 3>* dMatTiltdTauY = 0,
+                                     Matx<FLOAT, 3, 3>* invMatTilt = 0)
+    {
+        FLOAT cTauX = cos(tauX);
+        FLOAT sTauX = sin(tauX);
+        FLOAT cTauY = cos(tauY);
+        FLOAT sTauY = sin(tauY);
+        Matx<FLOAT, 3, 3> matRotX = Matx<FLOAT, 3, 3>(1,0,0,0,cTauX,sTauX,0,-sTauX,cTauX);
+        Matx<FLOAT, 3, 3> matRotY = Matx<FLOAT, 3, 3>(cTauY,0,-sTauY,0,1,0,sTauY,0,cTauY);
+        Matx<FLOAT, 3, 3> matRotXY = matRotY * matRotX;
+        Matx<FLOAT, 3, 3> matProjZ = Matx<FLOAT, 3, 3>(matRotXY(2,2),0,-matRotXY(0,2),0,matRotXY(2,2),-matRotXY(1,2),0,0,1);
+        if (matTilt)
+        {
+            // Matrix for trapezoidal distortion of tilted image sensor
+            *matTilt = matProjZ * matRotXY;
+        }
+        if (dMatTiltdTauX)
+        {
+            // Derivative with respect to tauX
+            Matx<FLOAT, 3, 3> dMatRotXYdTauX = matRotY * Matx<FLOAT, 3, 3>(0,0,0,0,-sTauX,cTauX,0,-cTauX,-sTauX);
+            Matx<FLOAT, 3, 3> dMatProjZdTauX = Matx<FLOAT, 3, 3>(dMatRotXYdTauX(2,2),0,-dMatRotXYdTauX(0,2),
+                                                                 0,dMatRotXYdTauX(2,2),-dMatRotXYdTauX(1,2),0,0,0);
+            *dMatTiltdTauX = (matProjZ * dMatRotXYdTauX) + (dMatProjZdTauX * matRotXY);
+        }
+        if (dMatTiltdTauY)
+        {
+            // Derivative with respect to tauY
+            Matx<FLOAT, 3, 3> dMatRotXYdTauY = Matx<FLOAT, 3, 3>(-sTauY,0,-cTauY,0,0,0,cTauY,0,-sTauY) * matRotX;
+            Matx<FLOAT, 3, 3> dMatProjZdTauY = Matx<FLOAT, 3, 3>(dMatRotXYdTauY(2,2),0,-dMatRotXYdTauY(0,2),
+                                                                 0,dMatRotXYdTauY(2,2),-dMatRotXYdTauY(1,2),0,0,0);
+            *dMatTiltdTauY = (matProjZ * dMatRotXYdTauY) + (dMatProjZdTauY * matRotXY);
+        }
+        if (invMatTilt)
+        {
+            FLOAT inv = 1./matRotXY(2,2);
+            Matx<FLOAT, 3, 3> invMatProjZ = Matx<FLOAT, 3, 3>(inv,0,inv*matRotXY(0,2),0,inv,inv*matRotXY(1,2),0,0,1);
+            *invMatTilt = matRotXY.t()*invMatProjZ;
+        }
+    }
+}
+
+static const char* cvDistCoeffErr = "Distortion coefficients must be 1x4, 4x1, 1x5, 5x1, 1x8, 8x1, 1x12, 12x1, 1x14 or 14x1 floating-point vector";
+
+static void _cvProjectPoints2Internal( const CvMat* objectPoints,
+                                      const CvMat* r_vec,
+                                      const CvMat* t_vec,
+                                      const CvMat* A,
+                                      const CvMat* distCoeffs,
+                                      CvMat* imagePoints, CvMat* dpdr CV_DEFAULT(NULL),
+                                      CvMat* dpdt CV_DEFAULT(NULL), CvMat* dpdf CV_DEFAULT(NULL),
+                                      CvMat* dpdc CV_DEFAULT(NULL), CvMat* dpdk CV_DEFAULT(NULL),
+                                      CvMat* dpdo CV_DEFAULT(NULL),
+                                      double aspectRatio CV_DEFAULT(0) )
+{
+    Ptr<CvMat> matM, _m;
+    Ptr<CvMat> _dpdr, _dpdt, _dpdc, _dpdf, _dpdk;
+    Ptr<CvMat> _dpdo;
+
+    int i, j, count;
+    int calc_derivatives;
+    const CvPoint3D64f* M;
+    CvPoint3D64f* m;
+    double r[3], R[9], dRdr[27], t[3], a[9], k[14] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0}, fx, fy, cx, cy;
+    Matx33d matTilt = Matx33d::eye();
+    Matx33d dMatTiltdTauX(0,0,0,0,0,0,0,-1,0);
+    Matx33d dMatTiltdTauY(0,0,0,0,0,0,1,0,0);
+    CvMat _r, _t, _a = cvMat( 3, 3, CV_64F, a ), _k;
+    CvMat matR = cvMat( 3, 3, CV_64F, R ), _dRdr = cvMat( 3, 9, CV_64F, dRdr );
+    double *dpdr_p = 0, *dpdt_p = 0, *dpdk_p = 0, *dpdf_p = 0, *dpdc_p = 0;
+    double* dpdo_p = 0;
+    int dpdr_step = 0, dpdt_step = 0, dpdk_step = 0, dpdf_step = 0, dpdc_step = 0;
+    int dpdo_step = 0;
+    bool fixedAspectRatio = aspectRatio > FLT_EPSILON;
+
+    if( !CV_IS_MAT(objectPoints) || !CV_IS_MAT(r_vec) ||
+        !CV_IS_MAT(t_vec) || !CV_IS_MAT(A) ||
+        /*!CV_IS_MAT(distCoeffs) ||*/ !CV_IS_MAT(imagePoints) )
+        CV_Error( CV_StsBadArg, "One of required arguments is not a valid matrix" );
+
+    int total = objectPoints->rows * objectPoints->cols * CV_MAT_CN(objectPoints->type);
+    if(total % 3 != 0)
+    {
+        //we have stopped support of homogeneous coordinates because it cause ambiguity in interpretation of the input data
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+    count = total / 3;
+
+    if( CV_IS_CONT_MAT(objectPoints->type) &&
+        (CV_MAT_DEPTH(objectPoints->type) == CV_32F || CV_MAT_DEPTH(objectPoints->type) == CV_64F)&&
+        ((objectPoints->rows == 1 && CV_MAT_CN(objectPoints->type) == 3) ||
+         (objectPoints->rows == count && CV_MAT_CN(objectPoints->type)*objectPoints->cols == 3) ||
+         (objectPoints->rows == 3 && CV_MAT_CN(objectPoints->type) == 1 && objectPoints->cols == count)))
+    {
+        matM.reset(cvCreateMat( objectPoints->rows, objectPoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(objectPoints->type)) ));
+        cvConvert(objectPoints, matM);
+    }
+    else
+    {
+//        matM = cvCreateMat( 1, count, CV_64FC3 );
+//        cvConvertPointsHomogeneous( objectPoints, matM );
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+
+    if( CV_IS_CONT_MAT(imagePoints->type) &&
+        (CV_MAT_DEPTH(imagePoints->type) == CV_32F || CV_MAT_DEPTH(imagePoints->type) == CV_64F) &&
+        ((imagePoints->rows == 1 && CV_MAT_CN(imagePoints->type) == 3) ||
+         (imagePoints->rows == count && CV_MAT_CN(imagePoints->type)*imagePoints->cols == 3) ||
+         (imagePoints->rows == 3 && CV_MAT_CN(imagePoints->type) == 1 && imagePoints->cols == count)))
+    {
+        _m.reset(cvCreateMat( imagePoints->rows, imagePoints->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(imagePoints->type)) ));
+        cvConvert(imagePoints, _m);
+    }
+    else
+    {
+//        _m = cvCreateMat( 1, count, CV_64FC2 );
+        CV_Error( CV_StsBadArg, "Homogeneous coordinates are not supported" );
+    }
+
+    M = (CvPoint3D64f*)matM->data.db;
+    m = (CvPoint3D64f*)_m->data.db;
+
+    if( (CV_MAT_DEPTH(r_vec->type) != CV_64F && CV_MAT_DEPTH(r_vec->type) != CV_32F) ||
+        (((r_vec->rows != 1 && r_vec->cols != 1) ||
+          r_vec->rows*r_vec->cols*CV_MAT_CN(r_vec->type) != 3) &&
+         ((r_vec->rows != 3 && r_vec->cols != 3) || CV_MAT_CN(r_vec->type) != 1)))
+        CV_Error( CV_StsBadArg, "Rotation must be represented by 1x3 or 3x1 "
+                                "floating-point rotation vector, or 3x3 rotation matrix" );
+
+    if( r_vec->rows == 3 && r_vec->cols == 3 )
+    {
+        _r = cvMat( 3, 1, CV_64FC1, r );
+        cvRodrigues2( r_vec, &_r );
+        cvRodrigues2( &_r, &matR, &_dRdr );
+        cvCopy( r_vec, &matR );
+    }
+    else
+    {
+        _r = cvMat( r_vec->rows, r_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(r_vec->type)), r );
+        cvConvert( r_vec, &_r );
+        cvRodrigues2( &_r, &matR, &_dRdr );
+    }
+
+    if( (CV_MAT_DEPTH(t_vec->type) != CV_64F && CV_MAT_DEPTH(t_vec->type) != CV_32F) ||
+        (t_vec->rows != 1 && t_vec->cols != 1) ||
+        t_vec->rows*t_vec->cols*CV_MAT_CN(t_vec->type) != 3 )
+        CV_Error( CV_StsBadArg,
+                  "Translation vector must be 1x3 or 3x1 floating-point vector" );
+
+    _t = cvMat( t_vec->rows, t_vec->cols, CV_MAKETYPE(CV_64F,CV_MAT_CN(t_vec->type)), t );
+    cvConvert( t_vec, &_t );
+
+    if( (CV_MAT_TYPE(A->type) != CV_64FC1 && CV_MAT_TYPE(A->type) != CV_32FC1) ||
+        A->rows != 3 || A->cols != 3 )
+        CV_Error( CV_StsBadArg, "Instrinsic parameters must be 3x3 floating-point matrix" );
+
+    cvConvert( A, &_a );
+    fx = a[0]; fy = a[4];
+    cx = a[2]; cy = a[5];
+
+    if( fixedAspectRatio )
+        fx = fy*aspectRatio;
+
+    if( distCoeffs )
+    {
+        if( !CV_IS_MAT(distCoeffs) ||
+            (CV_MAT_DEPTH(distCoeffs->type) != CV_64F &&
+             CV_MAT_DEPTH(distCoeffs->type) != CV_32F) ||
+            (distCoeffs->rows != 1 && distCoeffs->cols != 1) ||
+            (distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 4 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 5 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 8 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 12 &&
+             distCoeffs->rows*distCoeffs->cols*CV_MAT_CN(distCoeffs->type) != 14) )
+            CV_Error( CV_StsBadArg, cvDistCoeffErr );
+
+        _k = cvMat( distCoeffs->rows, distCoeffs->cols,
+                    CV_MAKETYPE(CV_64F,CV_MAT_CN(distCoeffs->type)), k );
+        cvConvert( distCoeffs, &_k );
+        if(k[12] != 0 || k[13] != 0)
+        {
+            _detail::computeTiltProjectionMatrix(k[12], k[13],
+                                                &matTilt, &dMatTiltdTauX, &dMatTiltdTauY);
+        }
+    }
+
+    if( dpdr )
+    {
+        if( !CV_IS_MAT(dpdr) ||
+            (CV_MAT_TYPE(dpdr->type) != CV_32FC1 &&
+             CV_MAT_TYPE(dpdr->type) != CV_64FC1) ||
+            dpdr->rows != count*2 || dpdr->cols != 3 )
+            CV_Error( CV_StsBadArg, "dp/drot must be 2Nx3 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdr->type) == CV_64FC1 )
+        {
+            _dpdr.reset(cvCloneMat(dpdr));
+        }
+        else
+            _dpdr.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
+        dpdr_p = _dpdr->data.db;
+        dpdr_step = _dpdr->step/sizeof(dpdr_p[0]);
+    }
+
+    if( dpdt )
+    {
+        if( !CV_IS_MAT(dpdt) ||
+            (CV_MAT_TYPE(dpdt->type) != CV_32FC1 &&
+             CV_MAT_TYPE(dpdt->type) != CV_64FC1) ||
+            dpdt->rows != count*2 || dpdt->cols != 3 )
+            CV_Error( CV_StsBadArg, "dp/dT must be 2Nx3 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdt->type) == CV_64FC1 )
+        {
+            _dpdt.reset(cvCloneMat(dpdt));
+        }
+        else
+            _dpdt.reset(cvCreateMat( 2*count, 3, CV_64FC1 ));
+        dpdt_p = _dpdt->data.db;
+        dpdt_step = _dpdt->step/sizeof(dpdt_p[0]);
+    }
+
+    if( dpdf )
+    {
+        if( !CV_IS_MAT(dpdf) ||
+            (CV_MAT_TYPE(dpdf->type) != CV_32FC1 && CV_MAT_TYPE(dpdf->type) != CV_64FC1) ||
+            dpdf->rows != count*2 || dpdf->cols != 2 )
+            CV_Error( CV_StsBadArg, "dp/df must be 2Nx2 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdf->type) == CV_64FC1 )
+        {
+            _dpdf.reset(cvCloneMat(dpdf));
+        }
+        else
+            _dpdf.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
+        dpdf_p = _dpdf->data.db;
+        dpdf_step = _dpdf->step/sizeof(dpdf_p[0]);
+    }
+
+    if( dpdc )
+    {
+        if( !CV_IS_MAT(dpdc) ||
+            (CV_MAT_TYPE(dpdc->type) != CV_32FC1 && CV_MAT_TYPE(dpdc->type) != CV_64FC1) ||
+            dpdc->rows != count*2 || dpdc->cols != 2 )
+            CV_Error( CV_StsBadArg, "dp/dc must be 2Nx2 floating-point matrix" );
+
+        if( CV_MAT_TYPE(dpdc->type) == CV_64FC1 )
+        {
+            _dpdc.reset(cvCloneMat(dpdc));
+        }
+        else
+            _dpdc.reset(cvCreateMat( 2*count, 2, CV_64FC1 ));
+        dpdc_p = _dpdc->data.db;
+        dpdc_step = _dpdc->step/sizeof(dpdc_p[0]);
+    }
+
+    if( dpdk )
+    {
+        if( !CV_IS_MAT(dpdk) ||
+            (CV_MAT_TYPE(dpdk->type) != CV_32FC1 && CV_MAT_TYPE(dpdk->type) != CV_64FC1) ||
+            dpdk->rows != count*2 || (dpdk->cols != 14 && dpdk->cols != 12 && dpdk->cols != 8 && dpdk->cols != 5 && dpdk->cols != 4 && dpdk->cols != 2) )
+            CV_Error( CV_StsBadArg, "dp/df must be 2Nx14, 2Nx12, 2Nx8, 2Nx5, 2Nx4 or 2Nx2 floating-point matrix" );
+
+        if( !distCoeffs )
+            CV_Error( CV_StsNullPtr, "distCoeffs is NULL while dpdk is not" );
+
+        if( CV_MAT_TYPE(dpdk->type) == CV_64FC1 )
+        {
+            _dpdk.reset(cvCloneMat(dpdk));
+        }
+        else
+            _dpdk.reset(cvCreateMat( dpdk->rows, dpdk->cols, CV_64FC1 ));
+        dpdk_p = _dpdk->data.db;
+        dpdk_step = _dpdk->step/sizeof(dpdk_p[0]);
+    }
+
+    if( dpdo )
+    {
+        if( !CV_IS_MAT( dpdo ) || ( CV_MAT_TYPE( dpdo->type ) != CV_32FC1
+                                    && CV_MAT_TYPE( dpdo->type ) != CV_64FC1 )
+            || dpdo->rows != count * 2 || dpdo->cols != count * 3 )
+            CV_Error( CV_StsBadArg, "dp/do must be 2Nx3N floating-point matrix" );
+
+        if( CV_MAT_TYPE( dpdo->type ) == CV_64FC1 )
+        {
+            _dpdo.reset( cvCloneMat( dpdo ) );
+        }
+        else
+            _dpdo.reset( cvCreateMat( 2 * count, 3 * count, CV_64FC1 ) );
+        cvZero(_dpdo);
+        dpdo_p = _dpdo->data.db;
+        dpdo_step = _dpdo->step / sizeof( dpdo_p[0] );
+    }
+
+    calc_derivatives = dpdr || dpdt || dpdf || dpdc || dpdk || dpdo;
+
+    for( i = 0; i < count; i++ )
+    {
+        double X = M[i].x, Y = M[i].y, Z = M[i].z;
+        double x = R[0]*X + R[1]*Y + R[2]*Z + t[0];
+        double y = R[3]*X + R[4]*Y + R[5]*Z + t[1];
+        double z = R[6]*X + R[7]*Y + R[8]*Z + t[2];
+        double r2, r4, r6, a1, a2, a3, cdist, icdist2;
+        double xd, yd, xd0, yd0, invProj;
+        Vec3d vecTilt;
+        Vec3d dVecTilt;
+        Matx22d dMatTilt;
+        Vec2d dXdYd;
+
+        double z0 = z;
+        z = z ? 1./z : 1;
+        x *= z; y *= z;
+
+        r2 = x*x + y*y;
+        r4 = r2*r2;
+        r6 = r4*r2;
+        a1 = 2*x*y;
+        a2 = r2 + 2*x*x;
+        a3 = r2 + 2*y*y;
+        cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
+        icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
+        xd0 = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
+        yd0 = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
+
+        // additional distortion by projecting onto a tilt plane
+        vecTilt = matTilt*Vec3d(xd0, yd0, 1);
+        invProj = vecTilt(2) ? 1./vecTilt(2) : 1;
+        xd = invProj * vecTilt(0);
+        yd = invProj * vecTilt(1);
+
+        m[i].x = xd*fx + cx;
+        m[i].y = yd*fy + cy;
+        m[i].z = z; // Just put the projected Z coordinate here, we mainly care about the sign
+
+        if( calc_derivatives )
+        {
+            if( dpdc_p )
+            {
+                dpdc_p[0] = 1; dpdc_p[1] = 0; // dp_xdc_x; dp_xdc_y
+                dpdc_p[dpdc_step] = 0;
+                dpdc_p[dpdc_step+1] = 1;
+                dpdc_p += dpdc_step*2;
+            }
+
+            if( dpdf_p )
+            {
+                if( fixedAspectRatio )
+                {
+                    dpdf_p[0] = 0; dpdf_p[1] = xd*aspectRatio; // dp_xdf_x; dp_xdf_y
+                    dpdf_p[dpdf_step] = 0;
+                    dpdf_p[dpdf_step+1] = yd;
+                }
+                else
+                {
+                    dpdf_p[0] = xd; dpdf_p[1] = 0;
+                    dpdf_p[dpdf_step] = 0;
+                    dpdf_p[dpdf_step+1] = yd;
+                }
+                dpdf_p += dpdf_step*2;
+            }
+            for (int row = 0; row < 2; ++row)
+                for (int col = 0; col < 2; ++col)
+                    dMatTilt(row,col) = matTilt(row,col)*vecTilt(2)
+                                        - matTilt(2,col)*vecTilt(row);
+            double invProjSquare = (invProj*invProj);
+            dMatTilt *= invProjSquare;
+            if( dpdk_p )
+            {
+                dXdYd = dMatTilt*Vec2d(x*icdist2*r2, y*icdist2*r2);
+                dpdk_p[0] = fx*dXdYd(0);
+                dpdk_p[dpdk_step] = fy*dXdYd(1);
+                dXdYd = dMatTilt*Vec2d(x*icdist2*r4, y*icdist2*r4);
+                dpdk_p[1] = fx*dXdYd(0);
+                dpdk_p[dpdk_step+1] = fy*dXdYd(1);
+                if( _dpdk->cols > 2 )
+                {
+                    dXdYd = dMatTilt*Vec2d(a1, a3);
+                    dpdk_p[2] = fx*dXdYd(0);
+                    dpdk_p[dpdk_step+2] = fy*dXdYd(1);
+                    dXdYd = dMatTilt*Vec2d(a2, a1);
+                    dpdk_p[3] = fx*dXdYd(0);
+                    dpdk_p[dpdk_step+3] = fy*dXdYd(1);
+                    if( _dpdk->cols > 4 )
+                    {
+                        dXdYd = dMatTilt*Vec2d(x*icdist2*r6, y*icdist2*r6);
+                        dpdk_p[4] = fx*dXdYd(0);
+                        dpdk_p[dpdk_step+4] = fy*dXdYd(1);
+
+                        if( _dpdk->cols > 5 )
+                        {
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r2, y*cdist*(-icdist2)*icdist2*r2);
+                            dpdk_p[5] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+5] = fy*dXdYd(1);
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r4, y*cdist*(-icdist2)*icdist2*r4);
+                            dpdk_p[6] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+6] = fy*dXdYd(1);
+                            dXdYd = dMatTilt*Vec2d(
+                                    x*cdist*(-icdist2)*icdist2*r6, y*cdist*(-icdist2)*icdist2*r6);
+                            dpdk_p[7] = fx*dXdYd(0);
+                            dpdk_p[dpdk_step+7] = fy*dXdYd(1);
+                            if( _dpdk->cols > 8 )
+                            {
+                                dXdYd = dMatTilt*Vec2d(r2, 0);
+                                dpdk_p[8] = fx*dXdYd(0); //s1
+                                dpdk_p[dpdk_step+8] = fy*dXdYd(1); //s1
+                                dXdYd = dMatTilt*Vec2d(r4, 0);
+                                dpdk_p[9] = fx*dXdYd(0); //s2
+                                dpdk_p[dpdk_step+9] = fy*dXdYd(1); //s2
+                                dXdYd = dMatTilt*Vec2d(0, r2);
+                                dpdk_p[10] = fx*dXdYd(0);//s3
+                                dpdk_p[dpdk_step+10] = fy*dXdYd(1); //s3
+                                dXdYd = dMatTilt*Vec2d(0, r4);
+                                dpdk_p[11] = fx*dXdYd(0);//s4
+                                dpdk_p[dpdk_step+11] = fy*dXdYd(1); //s4
+                                if( _dpdk->cols > 12 )
+                                {
+                                    dVecTilt = dMatTiltdTauX * Vec3d(xd0, yd0, 1);
+                                    dpdk_p[12] = fx * invProjSquare * (
+                                            dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
+                                    dpdk_p[dpdk_step+12] = fy*invProjSquare * (
+                                            dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
+                                    dVecTilt = dMatTiltdTauY * Vec3d(xd0, yd0, 1);
+                                    dpdk_p[13] = fx * invProjSquare * (
+                                            dVecTilt(0) * vecTilt(2) - dVecTilt(2) * vecTilt(0));
+                                    dpdk_p[dpdk_step+13] = fy * invProjSquare * (
+                                            dVecTilt(1) * vecTilt(2) - dVecTilt(2) * vecTilt(1));
+                                }
+                            }
+                        }
+                    }
+                }
+                dpdk_p += dpdk_step*2;
+            }
+
+            if( dpdt_p )
+            {
+                double dxdt[] = { z, 0, -x*z }, dydt[] = { 0, z, -y*z };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dr2dt = 2*x*dxdt[j] + 2*y*dydt[j];
+                    double dcdist_dt = k[0]*dr2dt + 2*k[1]*r2*dr2dt + 3*k[4]*r4*dr2dt;
+                    double dicdist2_dt = -icdist2*icdist2*(k[5]*dr2dt + 2*k[6]*r2*dr2dt + 3*k[7]*r4*dr2dt);
+                    double da1dt = 2*(x*dydt[j] + y*dxdt[j]);
+                    double dmxdt = (dxdt[j]*cdist*icdist2 + x*dcdist_dt*icdist2 + x*cdist*dicdist2_dt +
+                                    k[2]*da1dt + k[3]*(dr2dt + 4*x*dxdt[j]) + k[8]*dr2dt + 2*r2*k[9]*dr2dt);
+                    double dmydt = (dydt[j]*cdist*icdist2 + y*dcdist_dt*icdist2 + y*cdist*dicdist2_dt +
+                                    k[2]*(dr2dt + 4*y*dydt[j]) + k[3]*da1dt + k[10]*dr2dt + 2*r2*k[11]*dr2dt);
+                    dXdYd = dMatTilt*Vec2d(dmxdt, dmydt);
+                    dpdt_p[j] = fx*dXdYd(0);
+                    dpdt_p[dpdt_step+j] = fy*dXdYd(1);
+                }
+                dpdt_p += dpdt_step*2;
+            }
+
+            if( dpdr_p )
+            {
+                double dx0dr[] =
+                        {
+                                X*dRdr[0] + Y*dRdr[1] + Z*dRdr[2],
+                                X*dRdr[9] + Y*dRdr[10] + Z*dRdr[11],
+                                X*dRdr[18] + Y*dRdr[19] + Z*dRdr[20]
+                        };
+                double dy0dr[] =
+                        {
+                                X*dRdr[3] + Y*dRdr[4] + Z*dRdr[5],
+                                X*dRdr[12] + Y*dRdr[13] + Z*dRdr[14],
+                                X*dRdr[21] + Y*dRdr[22] + Z*dRdr[23]
+                        };
+                double dz0dr[] =
+                        {
+                                X*dRdr[6] + Y*dRdr[7] + Z*dRdr[8],
+                                X*dRdr[15] + Y*dRdr[16] + Z*dRdr[17],
+                                X*dRdr[24] + Y*dRdr[25] + Z*dRdr[26]
+                        };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dxdr = z*(dx0dr[j] - x*dz0dr[j]);
+                    double dydr = z*(dy0dr[j] - y*dz0dr[j]);
+                    double dr2dr = 2*x*dxdr + 2*y*dydr;
+                    double dcdist_dr = (k[0] + 2*k[1]*r2 + 3*k[4]*r4)*dr2dr;
+                    double dicdist2_dr = -icdist2*icdist2*(k[5] + 2*k[6]*r2 + 3*k[7]*r4)*dr2dr;
+                    double da1dr = 2*(x*dydr + y*dxdr);
+                    double dmxdr = (dxdr*cdist*icdist2 + x*dcdist_dr*icdist2 + x*cdist*dicdist2_dr +
+                                    k[2]*da1dr + k[3]*(dr2dr + 4*x*dxdr) + (k[8] + 2*r2*k[9])*dr2dr);
+                    double dmydr = (dydr*cdist*icdist2 + y*dcdist_dr*icdist2 + y*cdist*dicdist2_dr +
+                                    k[2]*(dr2dr + 4*y*dydr) + k[3]*da1dr + (k[10] + 2*r2*k[11])*dr2dr);
+                    dXdYd = dMatTilt*Vec2d(dmxdr, dmydr);
+                    dpdr_p[j] = fx*dXdYd(0);
+                    dpdr_p[dpdr_step+j] = fy*dXdYd(1);
+                }
+                dpdr_p += dpdr_step*2;
+            }
+
+            if( dpdo_p )
+            {
+                double dxdo[] = { z * ( R[0] - x * z * z0 * R[6] ),
+                                  z * ( R[1] - x * z * z0 * R[7] ),
+                                  z * ( R[2] - x * z * z0 * R[8] ) };
+                double dydo[] = { z * ( R[3] - y * z * z0 * R[6] ),
+                                  z * ( R[4] - y * z * z0 * R[7] ),
+                                  z * ( R[5] - y * z * z0 * R[8] ) };
+                for( j = 0; j < 3; j++ )
+                {
+                    double dr2do = 2 * x * dxdo[j] + 2 * y * dydo[j];
+                    double dr4do = 2 * r2 * dr2do;
+                    double dr6do = 3 * r4 * dr2do;
+                    double da1do = 2 * y * dxdo[j] + 2 * x * dydo[j];
+                    double da2do = dr2do + 4 * x * dxdo[j];
+                    double da3do = dr2do + 4 * y * dydo[j];
+                    double dcdist_do
+                            = k[0] * dr2do + k[1] * dr4do + k[4] * dr6do;
+                    double dicdist2_do = -icdist2 * icdist2
+                                         * ( k[5] * dr2do + k[6] * dr4do + k[7] * dr6do );
+                    double dxd0_do = cdist * icdist2 * dxdo[j]
+                                     + x * icdist2 * dcdist_do + x * cdist * dicdist2_do
+                                     + k[2] * da1do + k[3] * da2do + k[8] * dr2do
+                                     + k[9] * dr4do;
+                    double dyd0_do = cdist * icdist2 * dydo[j]
+                                     + y * icdist2 * dcdist_do + y * cdist * dicdist2_do
+                                     + k[2] * da3do + k[3] * da1do + k[10] * dr2do
+                                     + k[11] * dr4do;
+                    dXdYd = dMatTilt * Vec2d( dxd0_do, dyd0_do );
+                    dpdo_p[i * 3 + j] = fx * dXdYd( 0 );
+                    dpdo_p[dpdo_step + i * 3 + j] = fy * dXdYd( 1 );
+                }
+                dpdo_p += dpdo_step * 2;
+            }
+        }
+    }
+
+    if( _m != imagePoints )
+        cvConvert( _m, imagePoints );
+
+    if( _dpdr != dpdr )
+        cvConvert( _dpdr, dpdr );
+
+    if( _dpdt != dpdt )
+        cvConvert( _dpdt, dpdt );
+
+    if( _dpdf != dpdf )
+        cvConvert( _dpdf, dpdf );
+
+    if( _dpdc != dpdc )
+        cvConvert( _dpdc, dpdc );
+
+    if( _dpdk != dpdk )
+        cvConvert( _dpdk, dpdk );
+
+    if( _dpdo != dpdo )
+        cvConvert( _dpdo, dpdo );
+}
+
+static void _cvProjectPoints2( const CvMat* objectPoints,
+                        const CvMat* r_vec,
+                        const CvMat* t_vec,
+                        const CvMat* A,
+                        const CvMat* distCoeffs,
+                        CvMat* imagePoints, CvMat* dpdr,
+                        CvMat* dpdt, CvMat* dpdf,
+                        CvMat* dpdc, CvMat* dpdk,
+                        double aspectRatio )
+{
+    _cvProjectPoints2Internal( objectPoints, r_vec, t_vec, A, distCoeffs, imagePoints, dpdr, dpdt,
+                               dpdf, dpdc, dpdk, NULL, aspectRatio );
+}

aruco_pose/src/draw.h

@@ -4,3 +4,5 @@
 #include <opencv2/aruco.hpp>
 
 void _drawPlanarBoard(cv::aruco::Board *_board, cv::Size outSize, cv::OutputArray _img, int marginSize, int borderBits);
+void _drawAxis(cv::InputOutputArray image, cv::InputArray cameraMatrix, cv::InputArray distCoeffs,
+              cv::InputArray rvec, cv::InputArray tvec, float length);

aruco_pose/src/genmap.py

@@ -14,8 +14,6 @@ Options:
   <y>         Marker count along Y axis
   <dist_x>    Distance between markers along X axis
   <dist_y>    Distance between markers along Y axis
-  <sep_x>     Space beetween markers along X axis
-  <sep_y>     Space beetween markers along Y axis
   <first>     First marker ID
   --top-left  First marker is on top-left (not bottom-left)
 """
@@ -35,7 +33,7 @@ dist_x = float(arguments['<dist_x>'])
 dist_y = float(arguments['<dist_y>'])
 top_left = arguments['--top-left']
 
-max_y = markers_y * length
+max_y = (markers_y - 1) * dist_y
 
 for y in range(markers_y):
     for x in range(markers_x):

aruco_pose/src/utils.h

@@ -1,5 +1,6 @@
 #pragma once
 
+#include <cmath>
 #include <ros/ros.h>
 #include <tf/transform_datatypes.h>
 #include <geometry_msgs/Quaternion.h>
@@ -90,14 +91,33 @@ inline void fillTranslation(geometry_msgs::Vector3& translation, const cv::Vec3d
 	translation.z = tvec[2];
 }
 
-inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from)
+inline bool isFlipped(tf::Quaternion& q)
 {
-	tf::Quaternion q;
+	double yaw, pitch, roll;
-	q.setRPY(0, 0, -tf::getYaw(to) + tf::getYaw(from));
+	tf::Matrix3x3(q).getEulerYPR(yaw, pitch, roll);
-	tf::Quaternion pq;
+	return (abs(pitch) > M_PI / 2) || (abs(roll) > M_PI / 2);
-	tf::quaternionMsgToTF(from, pq);
+}
-	pq = pq * q;
+
-	tf::quaternionTFToMsg(pq, to);
+/* Set roll and pitch from "from" to "to", keeping yaw */
+inline void snapOrientation(geometry_msgs::Quaternion& to, const geometry_msgs::Quaternion& from, bool auto_flip = false)
+{
+	tf::Quaternion _from, _to;
+	tf::quaternionMsgToTF(from, _from);
+	tf::quaternionMsgToTF(to, _to);
+
+	if (auto_flip) {
+		if (!isFlipped(_from)) {
+			static const tf::Quaternion flip = tf::createQuaternionFromRPY(M_PI, 0, 0);
+			_from *= flip; // flip "from"
+		}
+	}
+
+	auto diff = tf::Matrix3x3(_to).transposeTimes(tf::Matrix3x3(_from));
+	double _, yaw;
+	diff.getRPY(_, _, yaw);
+	auto q = tf::createQuaternionFromRPY(0, 0, -yaw);
+	_from = _from * q; // set yaw from "to" to "from"
+	tf::quaternionTFToMsg(_from, to); // set "from" to "to"
 }
 
 inline void transformToPose(const geometry_msgs::Transform& transform, geometry_msgs::Pose& pose)

book.json

@@ -4,6 +4,7 @@
     "author": "Copter Express",
     "language": "ru",
     "root": "docs/",
+    "gitbook": "^3.2.2",
     "plugins": [
         "youtube",
         "richquotes@https://github.com/okalachev/gitbook-plugin-richquotes.git",

builder/assets/clever.service

@@ -7,7 +7,8 @@ After=roscore.service
 User=pi
 EnvironmentFile=/lib/systemd/system/roscore.env
 ExecStart=/opt/ros/kinetic/bin/roslaunch clever clever.launch --wait --screen
-Restart=on-abort
+Restart=on-failure
+RestartSec=3
 
 [Install]
 WantedBy=multi-user.target

builder/assets/clever_rename

@@ -0,0 +1,35 @@
+#!/usr/bin/env bash
+
+# Set Clever hostname to the specified value
+
+set -e
+
+NEW_NAME_OPT=$1
+
+if [[ -z ${NEW_NAME_OPT} ]]; then
+  echo "Please specify new name for this Clever"
+  exit 1
+fi
+
+NEW_NAME=$(echo ${NEW_NAME_OPT} | tr '[:upper:]' '[:lower:]')
+
+echo "Setting name to ${NEW_NAME}"
+
+echo "Backing up /etc/hostname"
+cp /etc/hostname /etc/hostname.bak
+echo "Writing new /etc/hostname"
+echo ${NEW_NAME} > /etc/hostname
+
+echo "Backing up /etc/hosts"
+cp /etc/hosts /etc/hosts.bak
+echo "Rewriting /etc/hosts with new values"
+sed -i 's/127\.0\.1\.1.*/127.0.1.1\t'${NEW_NAME}'/g' /etc/hosts
+
+echo "Changing hostname in /lib/systemd/system/roscore.env"
+sed -i 's/ROS_HOSTNAME=.*/ROS_HOSTNAME='${NEW_NAME}'.local/g' /lib/systemd/system/roscore.env
+
+echo "Changing hostname in .bashrc"
+sed -i 's/export ROS_HOSTNAME=.*/export ROS_HOSTNAME='${NEW_NAME}'.local/g' /home/pi/.bashrc
+
+echo "Done, reboot your Clever to see the results"
+

builder/assets/init_rpi.sh

@@ -32,7 +32,9 @@ echo_stamp() {
 }
 
 echo_stamp "Rename SSID"
-sudo sed -i.OLD "s/CLEVER/CLEVER-$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e 's/[^0-9]//g' | cut -c 1-4)/g" /etc/wpa_supplicant/wpa_supplicant.conf
+NEW_SSID='CLEVER-'$(head -c 100 /dev/urandom | xxd -ps -c 100 | sed -e "s/[^0-9]//g" | cut -c 1-4)
+sudo sed -i.OLD "s/CLEVER/${NEW_SSID}/" /etc/wpa_supplicant/wpa_supplicant.conf
+clever_rename ${NEW_SSID}
 
 echo_stamp "Harware setup"
 /root/hardware_setup.sh

builder/assets/ros_python_paths

@@ -0,0 +1,8 @@
+Defaults        env_keep += "PYTHONPATH"
+Defaults        env_keep += "PATH"
+Defaults        env_keep += "ROS_ROOT"
+Defaults        env_keep += "ROS_MASTER_URI"
+Defaults        env_keep += "ROS_PACKAGE_PATH"
+Defaults        env_keep += "ROS_LOCATIONS"
+Defaults        env_keep += "ROS_HOME"
+Defaults        env_keep += "ROS_LOG_DIR"

builder/assets/roscore.service

@@ -6,7 +6,8 @@ After=network.target
 User=pi
 EnvironmentFile=/lib/systemd/system/roscore.env
 ExecStart=/opt/ros/kinetic/bin/roscore
-Restart=on-abort
+Restart=on-failure
+RestartSec=3
 
 [Install]
 WantedBy=multi-user.target

builder/image-build.sh

@@ -11,7 +11,7 @@
 
 set -e # Exit immidiately on non-zero result
 
-SOURCE_IMAGE="https://downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2018-06-29/2018-06-27-raspbian-stretch-lite.zip"
+SOURCE_IMAGE="https://downloads.raspberrypi.org/raspbian_lite/images/raspbian_lite-2018-11-15/2018-11-13-raspbian-stretch-lite.zip"
 
 export DEBIAN_FRONTEND=${DEBIAN_FRONTEND:='noninteractive'}
 export LANG=${LANG:='C.UTF-8'}
@@ -108,9 +108,12 @@ ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever.
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.env' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/roscore.service' '/lib/systemd/system/'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-rosdep-clever.yaml' '/etc/ros/rosdep/'
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/ros_python_paths' '/etc/sudoers.d/'
 # ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/kinetic-ros-clever.rosinstall' '/home/pi/ros_catkin_ws/'
 # Add PX4 udev rules
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/99-px4fmu.rules' '/lib/udev/rules.d/'
+# Add rename script
+${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} copy ${SCRIPTS_DIR}'/assets/clever_rename' '/usr/local/bin/clever_rename'
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-ros.sh' ${REPO_URL} ${IMAGE_VERSION} false false ${NUMBER_THREADS}
 ${BUILDER_DIR}/image-chroot.sh ${IMAGE_PATH} exec ${SCRIPTS_DIR}'/image-validate.sh'
 

builder/image-ros.sh

@@ -42,9 +42,10 @@ echo_stamp() {
 my_travis_retry() {
   local result=0
   local count=1
-  while [ $count -le 3 ]; do
+  local max_count=50
+  while [ $count -le $max_count ]; do
     [ $result -ne 0 ] && {
-      echo -e "\n${ANSI_RED}The command \"$@\" failed. Retrying, $count of 3.${ANSI_RESET}\n" >&2
+      echo -e "\nThe command \"$@\" failed. Retrying, $count of $max_count.\n" >&2
     }
     # ! { } ignores set -e, see https://stackoverflow.com/a/4073372
     ! { "$@"; result=$?; }
@@ -53,21 +54,21 @@ my_travis_retry() {
     sleep 1
   done
 
-  [ $count -gt 3 ] && {
+  [ $count -gt $max_count ] && {
-    echo -e "\n${ANSI_RED}The command \"$@\" failed 3 times.${ANSI_RESET}\n" >&2
+    echo -e "\nThe command \"$@\" failed $max_count times.\n" >&2
   }
 
   return $result
 }
 
 # TODO: 'kinetic-rosdep-clever.yaml' should add only if we use our repo?
-echo_stamp "Init rosdep" \
+echo_stamp "Init rosdep"
-&& rosdep init \
+my_travis_retry rosdep init
-&& echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list \
+echo "yaml file:///etc/ros/rosdep/kinetic-rosdep-clever.yaml" >> /etc/ros/rosdep/sources.list.d/20-default.list
-&& rosdep update
+my_travis_retry rosdep update
 
 echo_stamp "Populate rosdep for ROS user"
-sudo -u pi rosdep update
+my_travis_retry sudo -u pi rosdep update
 
 resolve_rosdep() {
   # TEMPLATE: resolve_rosdep <CATKIN_PATH> <ROS_DISTRO> <OS_DISTRO> <OS_VERSION>
@@ -168,7 +169,8 @@ apt-get install -y --no-install-recommends \
     ros-kinetic-rosserial \
     ros-kinetic-usb-cam \
     ros-kinetic-vl53l1x \
-    ros-kinetic-opencv3=3.3.19-0stretch
+    ros-kinetic-opencv3=3.3.19-0stretch \
+    ros-kinetic-rosshow
 
 # TODO move GeographicLib datasets to Mavros debian package
 echo_stamp "Install GeographicLib datasets (needs for mavros)" \

builder/image-software.sh

@@ -91,7 +91,7 @@ tcpdump \
 ltrace \
 libpoco-dev=1.7.6+dfsg1-5+deb9u1 \
 python-rosdep \
-python-rosinstall-generator=0.1.14-1 \
+python-rosinstall-generator \
 python-wstool=0.1.17-1 \
 python-rosinstall=0.7.8-1 \
 build-essential=12.3 \
@@ -99,12 +99,17 @@ libffi-dev \
 monkey=1.6.9-1 \
 pigpio python-pigpio python3-pigpio \
 i2c-tools \
+espeak espeak-data python-espeak \
 ntpdate \
 python-dev \
 python3-dev \
+python-systemd \
 && echo_stamp "Everything was installed!" "SUCCESS" \
 || (echo_stamp "Some packages wasn't installed!" "ERROR"; exit 1)
 
+echo_stamp "Updating kernel to fix camera bug"
+apt-get install --no-install-recommends -y raspberrypi-kernel=1.20190401-1
+
 # Deny byobu to check available updates
 sed -i "s/updates_available//" /usr/share/byobu/status/status
 # sed -i "s/updates_available//" /home/pi/.byobu/status
@@ -113,6 +118,7 @@ echo_stamp "Installing pip"
 curl https://bootstrap.pypa.io/get-pip.py -o get-pip.py
 python3 get-pip.py
 python get-pip.py
+rm get-pip.py
 #my_travis_retry pip install --upgrade pip
 #my_travis_retry pip3 install --upgrade pip
 

builder/test/tests.py

@@ -25,6 +25,6 @@ import pymavlink
 from pymavlink import mavutil
 import rpi_ws281x
 import pigpio
+from espeak import espeak
 
 print cv2.getBuildInformation()
-

builder/test/tests.sh

@@ -28,6 +28,7 @@ monkey --version
 pigpiod -v
 i2cdetect -V
 butterfly -h
+espeak --version
 
 # ros stuff
 
@@ -46,3 +47,4 @@ rosversion rosserial
 rosversion usb_cam
 rosversion cv_camera
 rosversion web_video_server
+rosversion rosshow

clever/launch/clever.launch

@@ -63,7 +63,6 @@
     <!-- vl53l1x ToF rangefinder -->
     <node name="vl53l1x" pkg="vl53l1x" type="vl53l1x_node" output="screen" if="$(arg rangefinder_vl53l1x)">
         <param name="frame_id" value="rangefinder"/>
-        <param name="offset" value="-0.05"/>
         <remap from="~range" to="mavros/distance_sensor/rangefinder_sub"/> <!-- redirect data to FCU -->
     </node>
 

clever/src/optical_flow.cpp

@@ -161,7 +161,12 @@ private:
 			flow_camera.header.stamp = msg->header.stamp;
 			flow_camera.vector.x = flow_y; // +y means counter-clockwise rotation around Y axis
 			flow_camera.vector.y = -flow_x; // +x means clockwise rotation around X axis
-			tf_buffer_.transform(flow_camera, flow_fcu, fcu_frame_id_);
+			try {
+				tf_buffer_.transform(flow_camera, flow_fcu, fcu_frame_id_);
+			} catch (const tf2::TransformException& e) {
+				// transform is not available yet
+				return;
+			}
 
 			// Calculate integration time
 			ros::Duration integration_time = msg->header.stamp - prev_stamp_;

clever/src/selfcheck.py

@@ -1,19 +1,21 @@
 #!/usr/bin/env python
 
 import math
-from subprocess import Popen, PIPE
+import subprocess
 import re
 import traceback
+import numpy
 import rospy
 from std_srvs.srv import Trigger
 from sensor_msgs.msg import Image, CameraInfo, NavSatFix, Imu, Range
 from mavros_msgs.msg import State, OpticalFlowRad
-from geometry_msgs.msg import PoseStamped, TwistStamped
+from mavros_msgs.srv import ParamGet
+from geometry_msgs.msg import PoseStamped, TwistStamped, PoseWithCovarianceStamped
 import tf.transformations as t
+from aruco_pose.msg import MarkerArray
+from systemd import journal
 
 
-# TODO: roscore is running
-# TODO: clever.service is running
 # TODO: check attitude is present
 # TODO: disk free space
 # TODO: map, base_link, body
@@ -42,8 +44,10 @@ def check(name):
                 for f in failures:
                     rospy.logwarn('%s: %s', name, f)
             except Exception as e:
+                for f in failures:
+                    rospy.logwarn('%s: %s', name, f)
                 traceback.print_exc()
-                rospy.logwarn('%s: exception occurred', name)
+                rospy.logerr('%s: exception occurred', name)
                 return
             if not failures:
                 rospy.loginfo('%s: OK', name)
@@ -51,12 +55,49 @@ def check(name):
     return inner
 
 
+param_get = rospy.ServiceProxy('mavros/param/get', ParamGet)
+
+
+def get_param(name):
+    try:
+        res = param_get(param_id=name)
+    except rospy.ServiceException as e:
+        failure('%s: %s', name, str(e))
+        return None
+
+    if not res.success:
+        failure('Unable to retrieve PX4 parameter %s', name)
+    else:
+        if res.value.integer != 0:
+            return res.value.integer
+        return res.value.real
+
+
 @check('FCU')
 def check_fcu():
     try:
         state = rospy.wait_for_message('mavros/state', State, timeout=3)
         if not state.connected:
             failure('no connection to the FCU (check wiring)')
+
+        est = get_param('SYS_MC_EST_GROUP')
+        if est == 1:
+            rospy.loginfo('Selected estimator: LPE')
+            fuse = get_param('LPE_FUSION')
+            if fuse & (1 << 4):
+                rospy.loginfo('LPE_FUSION: land detector fusion is enabled')
+            else:
+                rospy.loginfo('LPE_FUSION: land detector fusion is disabled')
+            if fuse & (1 << 7):
+                rospy.loginfo('LPE_FUSION: barometer fusion is enabled')
+            else:
+                rospy.loginfo('LPE_FUSION: barometer fusion is disabled')
+
+        elif est == 2:
+            rospy.loginfo('Selected estimator: EKF2')
+        else:
+            failure('Unknown selected estimator: %s', est)
+
     except rospy.ROSException:
         failure('no MAVROS state (check wiring)')
 
@@ -95,6 +136,7 @@ def check_aruco():
 
 @check('Vision position estimate')
 def check_vpe():
+    vis = None
     try:
         vis = rospy.wait_for_message('mavros/vision_pose/pose', PoseStamped, timeout=1)
     except rospy.ROSException:
@@ -102,7 +144,45 @@ def check_vpe():
             vis = rospy.wait_for_message('mavros/mocap/pose', PoseStamped, timeout=1)
         except rospy.ROSException:
             failure('no VPE or MoCap messages')
-            return
+            # check if vpe_publisher is running
+            try:
+                subprocess.check_output(['pgrep', '-x', 'vpe_publisher'])
+            except subprocess.CalledProcessError:
+                return  # it's not running, skip following checks
+
+    # check PX4 settings
+    est = get_param('SYS_MC_EST_GROUP')
+    if est == 1:
+        ext_yaw = get_param('ATT_EXT_HDG_M')
+        if ext_yaw != 1:
+            failure('vision yaw is disabled, change ATT_EXT_HDG_M parameter')
+        vision_yaw_w = get_param('ATT_W_EXT_HDG')
+        if vision_yaw_w == 0:
+            failure('vision yaw weight is zero, change ATT_W_EXT_HDG parameter')
+        else:
+            rospy.loginfo('Vision yaw weight: %.2f', vision_yaw_w)
+        fuse = get_param('LPE_FUSION')
+        if not fuse & (1 << 2):
+            failure('vision position fusion is disabled, change LPE_FUSION parameter')
+        delay = get_param('LPE_VIS_DELAY')
+        if delay != 0:
+            failure('LPE_VIS_DELAY parameter is %s, but it should be zero', delay)
+        rospy.loginfo('LPE_VIS_XY is %.2f m, LPE_VIS_Z is %.2f m', get_param('LPE_VIS_XY'), get_param('LPE_VIS_Z'))
+    elif est == 2:
+        fuse = get_param('EKF2_AID_MASK')
+        if not fuse & (1 << 3):
+            failure('vision position fusion is disabled, change EKF2_AID_MASK parameter')
+        if not fuse & (1 << 4):
+            failure('vision yaw fusion is disabled, change EKF2_AID_MASK parameter')
+        delay = get_param('EKF2_EV_DELAY')
+        if delay != 0:
+            failure('EKF2_EV_DELAY is %.2f, but it should be zero', delay)
+        rospy.loginfo('EKF2_EVA_NOISE is %.3f, EKF2_EVP_NOISE is %.3f',
+                      get_param('EKF2_EVA_NOISE'),
+                      get_param('EKF2_EVP_NOISE'))
+
+    if not vis:
+        return
 
     # check vision pose and estimated pose inconsistency
     try:
@@ -173,7 +253,7 @@ def check_velocity():
             failure('vertical velocity estimation is %.2f m/s; is copter staying still?' % vert)
 
         angular = velocity.twist.angular
-        ANGULAR_VELOCITY_LIMIT = 0.01
+        ANGULAR_VELOCITY_LIMIT = 0.1
         if abs(angular.x) > ANGULAR_VELOCITY_LIMIT:
             failure('pitch rate estimation is %.2f rad/s (%.2f deg/s); is copter staying still?',
                     angular.x, math.degrees(angular.x))
@@ -200,6 +280,42 @@ def check_optical_flow():
     # TODO:check FPS!
     try:
         rospy.wait_for_message('mavros/px4flow/raw/send', OpticalFlowRad, timeout=0.5)
+
+        # check PX4 settings
+        rot = get_param('SENS_FLOW_ROT')
+        if rot != 0:
+            failure('SENS_FLOW_ROT parameter is %s, but it should be zero', rot)
+        est = get_param('SYS_MC_EST_GROUP')
+        if est == 1:
+            fuse = get_param('LPE_FUSION')
+            if not fuse & (1 << 1):
+                failure('optical flow fusion is disabled, change LPE_FUSION parameter')
+            if not fuse & (1 << 1):
+                failure('flow gyro compensation is disabled, change LPE_FUSION parameter')
+            scale = get_param('LPE_FLW_SCALE')
+            if not numpy.isclose(scale, 1.0):
+                failure('LPE_FLW_SCALE parameter is %.2f, but it should be 1.0', scale)
+
+            rospy.loginfo('LPE_FLW_QMIN is %s, LPE_FLW_R is %.4f, LPE_FLW_RR is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
+                          get_param('LPE_FLW_QMIN'),
+                          get_param('LPE_FLW_R'),
+                          get_param('LPE_FLW_RR'),
+                          get_param('SENS_FLOW_MINHGT'),
+                          get_param('SENS_FLOW_MAXHGT'))
+        elif est == 2:
+            fuse = get_param('EKF2_AID_MASK')
+            if not fuse & (1 << 1):
+                failure('optical flow fusion is disabled, change EKF2_AID_MASK parameter')
+            delay = get_param('EKF2_OF_DELAY')
+            if delay != 0:
+                failure('EKF2_OF_DELAY is %.2f, but it should be zero', delay)
+            rospy.loginfo('EKF2_OF_QMIN is %s, EKF2_OF_N_MIN is %.4f, EKF2_OF_N_MAX is %.4f, SENS_FLOW_MINHGT is %.3f, SENS_FLOW_MAXHGT is %.3f',
+                          get_param('EKF2_OF_QMIN'),
+                          get_param('EKF2_OF_N_MIN'),
+                          get_param('EKF2_OF_N_MAX'),
+                          get_param('SENS_FLOW_MINHGT'),
+                          get_param('SENS_FLOW_MAXHGT'))
+
     except rospy.ROSException:
         failure('no optical flow data (from Raspberry)')
 
@@ -207,21 +323,46 @@ def check_optical_flow():
 @check('Rangefinder')
 def check_rangefinder():
     # TODO: check FPS!
+    rng = False
     try:
-        rospy.wait_for_message('mavros/distance_sensor/rangefinder_3_sub', Range, timeout=0.5)
+        rospy.wait_for_message('mavros/distance_sensor/rangefinder_sub', Range, timeout=4)
+        rng = True
     except rospy.ROSException:
-        failure('no randefinder data from Raspberry')
+        failure('no rangefinder data from Raspberry')
+
     try:
-        rospy.wait_for_message('mavros/distance_sensor/rangefinder_0', Range, timeout=0.5)
+        rospy.wait_for_message('mavros/distance_sensor/rangefinder', Range, timeout=4)
+        rng = True
     except rospy.ROSException:
         failure('no rangefinder data from PX4')
 
+    if not rng:
+        return
+
+    est = get_param('SYS_MC_EST_GROUP')
+    if est == 1:
+        fuse = get_param('LPE_FUSION')
+        if not fuse & (1 << 5):
+            rospy.loginfo('"pub agl as lpos down" in LPE_FUSION is disabled, NOT operating over flat surface')
+        else:
+            rospy.loginfo('"pub agl as lpos down" in LPE_FUSION is enabled, operating over flat surface')
+
+    elif est == 2:
+        hgt = get_param('EKF2_HGT_MODE')
+        if hgt != 2:
+            rospy.loginfo('EKF2_HGT_MODE != Range sensor, NOT operating over flat surface')
+        else:
+            rospy.loginfo('EKF2_HGT_MODE = Range sensor, operating over flat surface')
+        aid = get_param('EKF2_RNG_AID')
+        if aid != 1:
+            rospy.loginfo('EKF2_RNG_AID != 1, range sensor aiding disabled')
+        else:
+            rospy.loginfo('EKF2_RNG_AID = 1, range sensor aiding enabled')
+
 
 @check('Boot duration')
 def check_boot_duration():
-    proc = Popen('systemd-analyze', stdout=PIPE)
+    output = subprocess.check_output('systemd-analyze')
-    proc.wait()
-    output = proc.communicate()[0]
     r = re.compile(r'([\d\.]+)s$')
     duration = float(r.search(output).groups()[0])
     if duration > 15:
@@ -232,9 +373,7 @@ def check_boot_duration():
 def check_cpu_usage():
     WHITELIST = 'nodelet',
     CMD = "top -n 1 -b -i | tail -n +8 | awk '{ printf(\"%-8s\\t%-8s\\t%-8s\\n\", $1, $9, $12); }'"
-    proc = Popen(CMD, stdout=PIPE, shell=True)
+    output = subprocess.check_output(CMD, shell=True)
-    proc.wait()
-    output = proc.communicate()[0]
     processes = output.split('\n')
     for process in processes:
         if not process:
@@ -246,7 +385,34 @@ def check_cpu_usage():
                     cpu.strip(), cmd.strip(), pid.strip())
 
 
+@check('clever.service')
+def check_clever_service():
+    output = subprocess.check_output('systemctl show -p ActiveState --value clever.service'.split())
+    if 'inactive' in output:
+        failure('clever.service is not running, try sudo systemctl restart clever')
+        return
+    j = journal.Reader()
+    j.this_boot()
+    j.add_match(_SYSTEMD_UNIT='clever.service')
+    j.add_disjunction()
+    j.add_match(UNIT='clever.service')
+    node_errors = []
+    r = re.compile(r'^(.*)\[(FATAL|ERROR)\] \[\d+.\d+\]: (.*)$')
+    for event in j:
+        msg = event['MESSAGE']
+        if ('Stopped Clever ROS package' in msg) or ('Started Clever ROS package' in msg):
+            node_errors = []
+        elif ('[ERROR]' in msg) or ('[FATAL]' in msg):
+            msg = r.search(msg).groups()[2]
+            if msg in node_errors:
+                continue
+            node_errors.append(msg)
+    for error in node_errors:
+        failure(error)
+
+
 def selfcheck():
+    check_clever_service()
     check_fcu()
     check_imu()
     check_local_position()

clever/src/simple_offboard.cpp

@@ -70,6 +70,7 @@ ros::Duration global_position_timeout;
 ros::Duration battery_timeout;
 float default_speed;
 bool auto_release;
+bool land_only_in_offboard;
 std::map<string, string> reference_frames;
 
 // Publishers
@@ -528,12 +529,13 @@ bool serve(enum setpoint_type_t sp_type, float x, float y, float z, float vx, fl
 			nav_speed = speed;
 		}
 
-		if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
+		// if (sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == POSITION || sp_type == VELOCITY) {
-			if (std::isnan(yaw) && yaw_rate == 0) {
+		// 	if (std::isnan(yaw) && yaw_rate == 0) {
-				// keep yaw unchanged
+		// 		// keep yaw unchanged
-				yaw = tf2::getYaw(local_position.pose.orientation);
+		//		// TODO: this is incorrect, because we need yaw in desired frame
-			}
+		// 		yaw = tf2::getYaw(local_position.pose.orientation);
-		}
+		// 	}
+		// }
 
 		if (sp_type == POSITION || sp_type == NAVIGATE || sp_type == NAVIGATE_GLOBAL || sp_type == VELOCITY || sp_type == ATTITUDE) {
 			// destination point and/or yaw
@@ -643,6 +645,12 @@ bool land(std_srvs::Trigger::Request& req, std_srvs::Trigger::Response& res)
 
 		checkState();
 
+		if (land_only_in_offboard) {
+			if (state.mode != "OFFBOARD") {
+				throw std::runtime_error("Copter is not in OFFBOARD mode");
+			}
+		}
+
 		static mavros_msgs::SetMode sm;
 		sm.request.custom_mode = "AUTO.LAND";
 
@@ -687,6 +695,7 @@ int main(int argc, char **argv)
 	nh.param<string>("mavros/local_position/tf/child_frame_id", fcu_frame, "base_link");
 	nh_priv.param("target_frame", target.child_frame_id, string("navigate_target"));
 	nh_priv.param("auto_release", auto_release, true);
+	nh_priv.param("land_only_in_offboard", land_only_in_offboard, true);
 	nh_priv.param("default_speed", default_speed, 0.5f);
 	nh_priv.getParam("reference_frames", reference_frames);
 

clever/src/vpe_publisher.cpp

@@ -116,7 +116,7 @@ int main(int argc, char **argv) {
 	nh_priv.param<string>("offset_frame_id", offset_frame_id, "");
 	nh_priv.param<string>("mavros/local_position/frame_id", local_frame_id, "map");
 	nh_priv.param<string>("mavros/local_position/tf/child_frame_id", child_frame_id, "base_link");
-	offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 5.0));
+	offset_timeout = ros::Duration(nh_priv.param("offset_timeout", 3.0));
 
 	if (!frame_id.empty()) {
 		ROS_INFO("vpe_publisher: using data from TF");

clever/www/aruco_map.html

@@ -0,0 +1,19 @@
+<!DOCTYPE html>
+<html>
+	<head>
+		<script type="text/javascript" src="js/three.min.js"></script>
+		<script type="text/javascript" src="js/eventemitter2.js"></script>
+		<script type="text/javascript" src="js/roslib.js"></script>
+		<script type="text/javascript" src="js/ros3d.js"></script>
+		<title>Aruco Map visualization</title>
+	</head>
+	<body>
+		<div id="viz"></div>
+		<script type="text/javascript" src="js/viz.js"></script>
+		<script>
+			setScene('aruco_map');
+			addArucoMap();
+			addAxes();
+		</script>
+	</body>
+</html>

clever/www/index.html

@@ -5,6 +5,7 @@
 	<li><a href="" id="wvs">View image topics</a> (<code>web_video_server</code>)</li>
 	<li><a href="" id="butterfly">Open web terminal</a> (<code>Butterfly</code>)</li>
 	<li><a href="viz.html">View 3D visualization</a> (<code>ros3djs</code>)</li>
+	<li><a href="aruco_map.html">3D visualization for markers map</a> (<code>ros3djs</code>)</li>
 </ul>
 
 <script type="text/javascript">

clever/www/js/viz.js

@@ -20,50 +20,75 @@ ros.on('close', function() {
 	titleEl.innerText = 'Disconnected';
 });
 
-var viewer = new ROS3D.Viewer({
+var viewer, tfClient;
-	divID: 'viz',
-	width: 1000,
-	height: 600,
-	antialias: true
-});
 
-var tfClient = new ROSLIB.TFClient({
+function setScene(fixedFrame) {
-	ros: ros,
+	viewer = new ROS3D.Viewer({
-	angularThres: 0.01,
+		divID: 'viz',
-	transThres: 0.01,
+		width: 1000,
-	rate: 10.0,
+		height: 600,
-	fixedFrame : 'map'
+		antialias: true
-});
+	});
 
-// vehicle markers
+	tfClient = new ROSLIB.TFClient({
-var vehicleMarkers = new ROS3D.MarkerArrayClient({
+		ros: ros,
-	ros: ros,
+		angularThres: 0.01,
-	tfClient: tfClient,
+		transThres: 0.01,
-	topic: '/vehicle_marker',
+		rate: 10.0,
-	rootObject: viewer.scene
+		fixedFrame : fixedFrame
-});
+	});
 
-// camera markers
+	var map = new ROS3D.Grid({
-var cameraMarkers = new ROS3D.MarkerArrayClient({
+		ros: ros,
-	ros: ros,
+		tfClient: tfClient,
-	tfClient: tfClient,
+		rootObject: viewer.scene
-	topic: '/main_camera/camera_markers',
+	});
-	rootObject: viewer.scene
-});
 
-// detected aruco markers
+	viewer.scene.add(map);
-var cameraMarkers = new ROS3D.MarkerArrayClient({
+}
-	ros: ros,
-	tfClient: tfClient,
-	topic: '/aruco_detect/visualization',
-	rootObject: viewer.scene
-});
 
-var map = new ROS3D.Grid({
+function addAxes() {
-	ros: ros,
+	var axes = new ROS3D.Axes({
-	tfClient: tfClient,
+		ros: ros,
-	// frameID: 'map',
+		tfClient: tfClient,
-	rootObject: viewer.scene
+		rootObject: viewer.scene
-});
+	});
+	viewer.scene.add(axes);
+}
 
-viewer.scene.add(map);
+function addVehicle() {
+	new ROS3D.MarkerArrayClient({
+		ros: ros,
+		tfClient: tfClient,
+		topic: '/vehicle_marker',
+		rootObject: viewer.scene
+	});
+}
+
+
+function addCamera() {
+	new ROS3D.MarkerArrayClient({
+		ros: ros,
+		tfClient: tfClient,
+		topic: '/main_camera/camera_markers',
+		rootObject: viewer.scene
+	});
+}
+
+function addAruco() {
+	new ROS3D.MarkerArrayClient({
+		ros: ros,
+		tfClient: tfClient,
+		topic: '/aruco_detect/visualization',
+		rootObject: viewer.scene
+	});
+}
+
+function addArucoMap() {
+	new ROS3D.MarkerArrayClient({
+		ros: ros,
+		tfClient: tfClient,
+		topic: '/aruco_map/visualization',
+		rootObject: viewer.scene
+	});
+}

clever/www/viz.html

@@ -10,5 +10,11 @@
 	<body>
 		<div id="viz"></div>
 		<script type="text/javascript" src="js/viz.js"></script>
+		<script>
+			setScene('map');
+			addVehicle();
+			addCamera();
+			addAruco();
+		</script>
 	</body>
 </html>

docs/en/SUMMARY.md

@@ -43,12 +43,15 @@
   * [Working with a LED strip on Raspberry 3](leds.md)
   * [Using rviz and rqt](rviz.md)
   * [Working with the ultrasonic distance gage](sonar.md)
+  * [Working with a laser rangefinder](laser.md)
   * [PX4 Simulation](sitl.md)
   * [Software autorun](autolaunch.md)
   * [Controlling the copter from Arduino](arduino.md)
   * [Using an external 3G modem](3g.md)
 * Clever-based projects
   * [Copter spheric guard](shield.md)
+  * [Face recognition system](face_recognition.md)
+  * [An Android transmitter](android.md)
   * [Copter Hack 2018](copterhack2018.md)
   * [Copter Hack 2017](copterhack2017.md)
 * Supplementary materials
@@ -56,5 +59,7 @@
   * [Flashing ESCs using BLHeliSuite](esc_firmware.md)
   * [MAVLink](mavlink.md)
   * [PX4 Logs and Topics](flight_logs.md)
+  * [Camera calibration](calibration.md)
+  * [Working with IR sensors on Raspberry Pi 3](ir_sensors.md)
 * Textbook
   * [Theory and Videos](lessons.md)

docs/en/android.md

@@ -0,0 +1,141 @@
+# An Android transmitter
+
+As early as in the frosty January 2018, all owners of Apple mobile devices got a nice Wi-Fi piloting app for iOS. And now, a year later, such an application is available for another operating system. The latest version may be downloaded [**here**](https://vk.com/away.php?to=https%3A%2F%2Fplay.google.com%2Fstore%2Fapps%2Fdetails%3Fid%3Dexpress.copter.cleverrc&cc_key=).
+
+## Introduction
+
+In this article, I will tell you how to write your own or modify an existing transmitter for Android yourself. We will use the popular language *Kotlin*, and we will use *Android Studio* for an IDE. For those who never used it, I recommend reading the following [*materials*](https://www.google.com/search?ei=xQxDXMH0C8OOmgW4mYigDQ&q=%D0%A7%D1%82%D0%BE+%D0%B4%D0%B5%D0%BB%D0%B0%D1%82%D1%8C+%D0%B5%D1%81%D0%BB%D0%B8+%D1%8F+%D0%BD%D0%B5+%D1%83%D0%BC%D0%B5%D1%8E+%D0%BF%D0%B8%D1%81%D0%B0%D1%82%D1%8C+%D0%BF%D0%BE%D0%B4+%D0%B0%D0%BD%D0%B4%D1%80%D0%BE%D0%B8%D0%B4%3F&oq=%D0%A7%D1%82%D0%BE+%D0%B4%D0%B5%D0%BB%D0%B0%D1%82%D1%8C+%D0%B5%D1%81%D0%BB%D0%B8+%D1%8F+%D0%BD%D0%B5+%D1%83%D0%BC%D0%B5%D1%8E+%D0%BF%D0%B8%D1%81%D0%B0%D1%82%D1%8C+%D0%BF%D0%BE%D0%B4+%D0%B0%D0%BD%D0%B4%D1%80%D0%BE%D0%B8%D0%B4%3F&gs_l=psy-ab.3...4413.17423..17726...9.0..2.442.4577.45j5j1j0j1....2..0....1..gws-wiz.....6..0i71j35i39j0i131j0j0i67j0i131i67j0i22i30j33i22i29i30j33i21j33i160.0bZz-WGxoHY). The entire application code can be found [**here**](https://github.com/Tennessium/android). If you want to immediately get an app to further tuning, run the following command:
+
+```Bash
+git clone https://github.com/Tennessium/android
+```
+
+However, to make you fully understand the application, I will tell you about each stage of the project, as if you were building it from scratch.
+
+## Wrapper
+
+Let's start with the simplest thing — the appearance of our application. At [**GitHub**](https://github.com/CopterExpress/clever/tree/master/apps/android/app/src/main/assets), you can find *HTML*, *CSS* and *JavaScript* files, which make up the web page to be used for controlling the copter. To have this page displayed in our application, do the following:
+
+1. Create folder **assets** in the main folder of the app named **app**
+
+2. Add to it all files from [here](https://github.com/CopterExpress/clever/tree/master/apps/android/app/src/main/assets)
+
+If you reached this stage, you already have the web page you want, congratulations! Now we have to display it somehow in the app. To do this, in class *activity* in method **onCreate**, write the following code:
+
+```Kotlin
+main_web.loadUrl("file:///android_asset/index.html")
+```
+
+Where *main_web* is the ID of your *WebView*, which is in the *xml* file of the *activity* selected by you.
+
+Unfortunately, the quadcopter transmitter requires the entire screen of the device, while the interface elements of the system interfere with full-fledged use of the program. For this purpose, at the beginning of method **onCreate**, call the following function:
+
+```Kotlin
+private fun fullScreenCall() {
+	window.setFlags(WindowManager.LayoutParams.FLAG_FULLSCREEN, WindowManager.LayoutParams.FLAG_FULLSCREEN)
+	if (Build.VERSION.SDK_INT < 19) {
+		val v = this.window.decorView
+		v.systemUiVisibility = View.GONE
+	} else {
+		//for higher API versions.
+		val decorView = window.decorView
+		val uiOptions = View.SYSTEM_UI_FLAG_HIDE_NAVIGATION or View.SYSTEM_UI_FLAG_IMMERSIVE_STICKY
+		decorView.systemUiVisibility = uiOptions
+	}
+}
+```
+
+This feature allows getting rid of the system interface elements. Let's go ahead.
+
+This is how the transmitter looks at this stage:
+
+<img src="../assets/IMG_4397.PNG" width="50%">
+
+If you run your application, you will see that the sticks are not functioning. This is due to the fact that *JavaScript* is disabled in our page. To enable it, write the following code:
+
+```Kotlin
+	main_web.settings.apply {
+	domStorageEnabled = true
+	javaScriptEnabled = true
+	loadWithOverviewMode = true
+	useWideViewPort = true
+	setSupportZoom(false)
+}
+```
+
+This piece of code allows the page to use *JavaScript* and at the same time prepares for the next stage - **logics**.
+
+## Receiving data from the web page
+
+To let your phone receive data from the *HTML page*, create a class for interacting with the web interface
+
+```Kotlin
+class WebAppInterface(c: Context) {
+	@JavascriptInterface
+	public fun postMessage(message: String) {
+		val data = JSONObject(message)
+		send("255.255.255.255", 35602, pack(
+		data.getInt("x").toShort(),
+		data.getInt("y").toShort(),
+		data.getInt("z").toShort(),
+		data.getInt("r").toShort()))
+	}
+}
+```
+
+This class will receive messages from the web page sent by the *postMessage* where argument *message* is the message from the page.
+
+Now we have to link classes **WebAppInterface** and **MainActivity**. For this you have to add just one line to method **onCreate**:
+
+```Kotlin
+main_web.addJavascriptInterface(WebAppInterface(this), "appInterface")
+```
+
+## Sending data to the copter
+
+**Important!**
+For working in Internet in the *Android* platform, add the following line to tag *manifest* in file **AndroidManifest.xml**:
+
+```XML
+<uses-permission android:name="android.permission.INTERNET"/>
+```
+
+It will grant your application access to the Internet, and the ability to send data via **Wi-Fi**. And you will now learn how to do that. Let's go ahead.
+
+You have probably noticed function *send* in class **WebAppInterface**. It is this function that sends data to the copter. Let's declare it **outside classes**:
+
+```Kotlin
+fun send(host: String, port: Int, data: ByteArray, senderPort: Int = 0): Boolean {
+	var ret = false
+	var socket: DatagramSocket? = null
+	try {
+		socket = DatagramSocket(senderPort)
+		val address = InetAddress.getByName(host)
+		val packet = DatagramPacket(data, data.size, address, port)
+		socket.send(packet)
+		ret = true
+	} catch (e: Exception) {
+		e.printStackTrace()
+	} finally {
+		socket?.close()
+	}
+	return ret
+}
+```
+
+This function sends data via the [*user datagram protocol*](https://www.google.com/search?q=udp+%D0%BF%D1%80%D0%BE%D1%82%D0%BE%D0%BA%D0%BE%D0%BB&oq=udp+&aqs=chrome.0.69i59j69i57j35i39j0l3.1434j1j7&sourceid=chrome&ie=UTF-8) to the copter. The program sends **bytes**, so it would be a good idea to declare the function for creating an array of **bytes** from four variables:
+
+```Kotlin
+fun pack(x: Short, y: Short, z: Short, r: Short): ByteArray {
+	val pump_on_buf: ByteBuffer = ByteBuffer.allocate(8)
+	pump_on_buf.putShort(r)
+	pump_on_buf.putShort(z)
+	pump_on_buf.putShort(y)
+	pump_on_buf.putShort(x)
+	return pump_on_buf.array().reversedArray()
+}
+```
+
+## Summary
+
+Now your app has the full functionality of its analog for **iOS**. You can customize it as you wish. For any questions about the app, contact us in Telegram @Tenessinum.

docs/en/autolaunch.md

@@ -56,4 +56,4 @@ When scripting languages are used, [shebang] should be placed at the beginning o
 
 ```(bash)
 #!/usr/bin/env python
-```
+```

docs/en/calibratePID.md

@@ -13,4 +13,4 @@ The Rate Pitch and Rate Roll parameters should be the same.
 
 YAW parameters should be changed individually, according to the above instruction (usually the yaw doesn't require serious adjustment, you may leave it default).
 
-![ROLL oscillations](../assets/oscillRoll.jpg)
+![ROLL oscillations](../assets/oscillRoll.jpg)

docs/en/calibration.md

@@ -0,0 +1,228 @@
+# Camera calibration
+
+Computer vision is becoming more and more widespread. Often, computer vision algorithms are not precise and obtain distorted images from the camera, which is especially true for fisheye cameras.
+
+![img](../assets/img1.jpg)
+
+> The image is "rounded" closer to the edge.
+
+Any computer vision algorithm will perceive the picture incorrectly. To remove such distortion, the camera that receives the image is to be calibrated in accordance with its own peculiarities.
+
+## Script installation
+
+First, you have to install the necessary libraries:
+
+```
+pip install numpy
+pip install opencv-python
+pip install glob
+pip install pyyaml
+pip install urllib.request
+```
+
+Then download the script from the repository:
+
+```(bash)
+git clone https://github.com/tinderad/clever_cam_calibration.git
+```
+
+Go to the downloaded folder and install the script:
+
+```(bash)
+cd clever_cam_calibration
+sudo python setup.py build
+sudo python setup.py install
+```
+
+If you are using Windows, download the archive from the [repository](https://github.com/tinderad/clever_cam_calibration/archive/master.zip), unzip it and install:
+
+```(bash)
+cd path\to\archive\clever_cam_calibration\
+python setup.py build
+python setup.py install
+```
+
+> path\to\archive – path to unpacked archive.
+
+## Preparing for calibration
+
+You will have to prepare a calibration target. It looks like a chessboard. The file is available for downloading [here](https://www.oreilly.com/library/view/learning-opencv-3/9781491937983/assets/lcv3_ac01.png).
+Glue a printed target to any solid surface. Count the number of intersections on the board lengthwise and widthwise, measure the size of a cell (mm).
+
+![img](../assets/chessboard.jpg)
+
+Turn on Clever and connect to its Wi-Fi.
+
+> Navigate to 192.168.11.1:8080 and check whether the computer receives images from the image_raw topic.
+
+## Calibration
+
+Run script **_calibrate_cam_**:
+
+**Windows:**
+
+ ```(bash)
+>path\to\python\Scripts\calibrate_cam.exe
+```
+
+> path\to\Python – path to the Python folder
+
+**Linux:**
+
+```(bash)
+>calibrate_cam
+```
+
+Specify board parameters:
+
+```(bash)
+>calibrate_cam
+Chessboard width: # Intersections widthwise
+Chessboard height: # Intersections heightwise
+Square size: # Length of cell edge (mm)
+Saving mode (YES - on): # Save mode
+```
+
+> Save mode: if enabled, all received pictures will be saved in the current folder.
+
+The script will start running:
+
+```
+Calibration started!
+Commands:
+help, catch (key: Enter), delete, restart, stop, finish
+```
+
+To calibrate the camera, make at least 25 photos of the chessboard at various angles.
+
+![img](../assets/calibration.jpg)
+
+To make a photo, enter command **_catch_**.
+
+```(bash)
+>catch
+```
+
+The program will inform you about the calibration status.
+
+```(bash)
+...
+Chessboard not found, now 0 (25 required)
+>  # Enter
+---
+Image added, now 1 (25 required)
+```
+
+> Instead of entering command **_catch_** each time, you can just press **_Enter_** (enter a blank line).
+
+After you have made a sufficient number of images, enter command **_finish_**.
+
+```(bash)
+...
+>finish
+Calibration successful!
+```
+
+### Calibration by the existing images
+
+If you already have images, you can calibrate the camera by them with the help of script **_calibrate_cam_ex_**.
+
+```(bash)
+>calibrate_cam_ex
+```
+
+Specify target characteristics and the path to the folder with images:
+
+```(bash)
+>calibrate_cam_ex
+Chessboard width: # Intersections widthwise
+Chessboard height: # Intersections heightwise
+Square size: # Length of cell edge (mm)
+Path: # Path to the folder with images
+```
+
+Apart from that, this script works similarly to **_calibrate_cam_**.
+
+The program will process all received pictures, and create file **_camera_info_****_._****_yaml_** in the current folder. Using this file, you can equalize distortions in the images obtained from this camera.
+
+> If you change the resolution of the received image, you will have to re-calibrate the camera.
+
+## Correcting distortions
+
+Function **_get_undistorted_image(cv2_image, camera_info)_** is responsible for obtaining a corrected image:
+
+* **_cv2_image_**: An image encoded into a cv2 array.
+* **_camera_****___****_info_**: The path to the calibration file.¬
+
+The function returns a cv2 array, into which the corrected image is coded.
+
+> If you are using a fisheye camera provided with Clever, for processing images with resolution 320x240 or 640x480, you can use the existing calibration settings. To do this, pass parameters **_clever_cam_calibration.clevercamcalib.CLEVER_FISHEYE_CAM_320_** or **_clever_cam_calibration.clevercamcalib.CLEVER_FISHEYE_CAM_640_** as argument **_camera_info_**, respectively.
+
+## Examples of operation
+
+Source images:
+
+![img](../assets/img1.jpg)
+
+![img](../assets/img2.jpg)
+
+Corrected images:
+
+![img](../assets/calibresult.jpg)
+
+![img](../assets/calibresult1.jpg)
+
+## An example of usage
+
+**Processing image stream from the camera**.
+
+This program receives images from the camera on Clever and displays them on the screen in corrected for, using the existing calibration file.
+
+```python
+import clevercamcalib.clevercamcalib as ccc
+import cv2
+import urllib.request
+import numpy as np
+while True:
+	req = urllib.request.urlopen('http://192.168.11.1:8080/snapshot?topic=/main_camera/image_raw')
+    arr = np.asarray(bytearray(req.read()), dtype=np.uint8)
+    image = cv2.imdecode(arr, -1)
+    undistorted_img = ccc.get_undistorted_image(image, ccc.CLEVER_FISHEYE_CAM_640)
+    cv2.imshow("undistort", undistorted_img)
+    cv2.waitKey(33)
+cv2.destroyAllWindows()
+```
+
+## The usage for ArUco
+
+To apply the calibration parameters to the ArUco navigation system, move the calibration .yaml file to Raspberry Pi of Clever, and initialize it.
+
+> Don't forget to connect to Wi-Fi of Clever.
+
+The SFTP protocol is used for transferring the file. This example, WinSCP program is used.
+
+Connect to Raspberry Pi via SFTP:
+
+> Password: _**raspberry**_
+
+![img](../assets/wcp1.png)
+
+Press “Enter”. Go to _**/home/pi/catkin_ws/src/clever/clever/camera_info/**_, and copy the calibration .yaml file to this folder:
+
+![img](../assets/wcp2.jpg)
+
+Now we have to select this file in ArUco configuration. Connection via SSH is used for this purpose. This example, PuTTY program is used.
+
+Connect to Raspberry Pi via SSH:
+
+![img](../assets/pty1.jpg)
+
+Log in with username _**pi**_ and password _**raspberry**_, go to directory _**/home/pi/catkin_ws/src/clever/clever/launch**_ and start editing configuration _**main_camera.launch**_:
+
+![img](../assets/pty2.jpg)
+
+In line _**camera node**_, change parameter _**camera_info**_ to _**camera_info.yaml**_:
+
+![img](../assets/pty3.jpg)
+
+> Don't forget to change camera resolution.

docs/en/camera_frame.md

@@ -60,4 +60,4 @@ The first image — how a copter model looks in rviz with these settings, the se
 ```
 
 <img src="../assets/camera_option_4_rviz.png" width=400>
-<img src="../assets/camera_option_4_clever.jpg" width=400>
+<img src="../assets/camera_option_4_clever.jpg" width=400>

docs/en/copterhack2018.md

@@ -34,4 +34,4 @@ Winning teams:
 4. International Post (Novosibirsk) — automatic scattering leaflets from the drone.
 5. LAMAR (Yekaterinburg) — an automatic quadcopter battery replacement station.
 
-<img src="../assets/alcopter.jpg" title="Alcopter Team" height=300px>
+<img src="../assets/alcopter.jpg" title="Alcopter Team" height=300px>

docs/en/face_recognition.md

@@ -0,0 +1,163 @@
+# Face recognition system
+
+## Introduction
+
+Recently, face recognition systems have been getting a wider use, the application scope of this technology is really expansive: from regular selfie drones to police drones. Everywhere it is being integrated into various devices. The recognition process itself is really fascinating, and that's what inspired me to create a project associated with it.  The purpose of my internship project was to create a simple open source system for face recognition with a Clever quadcopter. The program takes images from the quadcopter's camera and processes it on a PC. Therefore, all other instructions are executed on a PC.
+
+## Development
+
+The first task was finding a recognition algorithm. As a solution to the problem, [a ready API for Python](https://github.com/ageitgey/face_recognition) was chosen. This API combines several advantages: recognition speed and accuracy, and ease of use.
+
+## Installation
+
+First, you have to install all the necessary libraries:
+
+```(bash)
+pip install face_recognition
+pip install opencv-python
+```
+
+Then download the script from the repository:
+
+```(bash)
+git clone https://github.com/mmkuznecov/face_recognition_from_clever.git
+```
+
+## Code explanation
+
+Enable libraries:
+
+```python
+import face_recognition
+import cv2
+import os
+import urllib.request
+import numpy as np
+```
+
+***This part of the code is intended for Python 3. In Python 2.7, enable urllib2 instead of urllib:***
+
+```python
+import urllib2
+```
+
+Create a list of encodings for images and a list of names:
+
+```python
+faces_images=[]
+for i in os.listdir('faces/'):
+    faces_images.append(face_recognition.load_image_file('faces/'+i))
+known_face_encodings=[]
+for i in faces_images:
+    known_face_encodings.append(face_recognition.face_encodings(i)[0])
+known_face_names=[]url
+for i in os.listdir('faces/'):
+    i=i.split('.')[0]
+    known_face_names.append(i)
+```
+
+***Addition: all images are stored in folder faces in format name.jpg***
+
+<img src="../assets/screen.jpg" width="50%">
+
+<img src="../assets/Mikhail.jpg" width="30%">
+
+<img src="../assets/Timofey.jpg" width="30%">
+
+Initialize some variables:
+
+```python
+face_locations = []
+face_encodings = []
+face_names = []
+process_this_frame = True
+```
+
+Get the image from the server, and convert it to format cv2:
+
+```python
+req = urllib.request.urlopen('http://192.168.11.1:8080/snapshot?topic=/main_camera/image_raw')
+arr = np.asarray(bytearray(req.read()), dtype=np.uint8)
+frame = cv2.imdecode(arr, -1)
+```
+
+***For Python 2.7:***
+
+```python
+req = urllib2.urlopen('http://192.168.11.1:8080/snapshot?topic=/main_camera/image_raw')
+arr = np.asarray(bytearray(req.read()), dtype=np.uint8)
+frame = cv2.imdecode(arr, -1)
+```
+
+Further explanation of the code is available at GitHub of the used API in the comments to [the next script](https://github.com/ageitgey/face_recognition/blob/master/examples/facerec_from_webcam_faster.py)
+
+## Using
+
+It is enough to connect to "Clever" via Wi-Fi and check whether the video stream from the camera is working correctly.
+
+Then just run the script:
+
+```(bash)
+python recog.py
+```
+
+And the output:
+
+<img src="../assets/Mikhail_output.jpg" width="50%">
+
+<img src="../assets/Timofey_output.jpg" width="50%">
+
+## Possible difficulties
+
+When the script is started, the following error may pop up:
+
+```python
+    known_face_encodings.append(face_recognition.face_encodings(i)[0])
+IndexError: list index out of range
+```
+
+In this case, try to edit the images in folder faces, perhaps the program cannot recognize faces in the images due to poor quality.
+
+## Using the calibration
+
+To improve recognition accuracy, you can use camera calibration. The calibration module may be installed using [a special package](https://github.com/tinderad/clever_cam_calibration). Instructions for installation and use are available in file calibration.md. The program that uses the calibration package is named recog_undist.py
+
+**Code brief explanation:**
+
+Enable installed package:
+
+```python
+import clever_cam_calibration.clevercamcalib as ccc
+```
+
+Add the following lines:
+
+```python
+height_or, width_or, depth_or = frame.shape
+```
+
+This way, you will obtain information about image size, where height_or is the height of the initial image in pixels, and width_or is the width of the initial image.
+Then correct distortions in the initial image, and get its parameters:
+
+```python
+if height_or==240 and width_or==320:
+    frame=ccc.get_undistorted_image(frame,ccc.CLEVER_FISHEYE_CAM_320)
+elif height_or==480 and width_or==640:
+    frame=ccc.get_undistorted_image(frame,ccc.CLEVER_FISHEYE_CAM_640)
+else:
+    frame=ccc.get_undistorted_image(frame,input("Input your path to the .yaml file: "))
+height_unz, width_unz, depth_unz = frame.shape
+```
+
+***In this case, we pass argument ссс.CLEVER_FISHEYE_CAM_640, since the resolution of the image in this example, is 640x480; you can also use ссс.CLEVER_FISHEYE_CAM_320 for resolution 320x240, otherwise you will have to send the path to the .yaml calibration file as the second argument.***
+
+Finally, return the image to its initial size:
+
+```python
+frame=cv2.resize(frame,(0,0), fx=(width_or/width_unz),fy=(height_or/height_unz))
+```
+
+This was, you can significantly improve recognition accuracy since the image processed will not be so badly distorted.
+
+<img src="../assets/misha_calib.jpg" width="50%">
+<img src="../assets/tim_calib.jpg" width="50%">

docs/en/firmware.md

@@ -50,4 +50,4 @@ To upload the `v3` firmware to Pixhawk, you may need the `force_upload` command:
 
 ```
 make px4fmu-v3_default force-upload
-```
+```

docs/en/fpv.md

@@ -46,4 +46,4 @@ The following may be used as fastening materials:
 1. Hot-melt glue;
 1. electrical tape;
 1. zip-ties (clamps);
-1. double-sided adhesive tape.
+1. double-sided adhesive tape.