<commit_before>/*****************************************************************************
*   MarkerDetector.cpp
*   Example_MarkerBasedAR
******************************************************************************
*   by Khvedchenia Ievgen, 5th Dec 2012
*   http://computer-vision-talks.com
******************************************************************************
*   Ch2 of the book "Mastering OpenCV with Practical Computer Vision Projects"
*   Copyright Packt Publishing 2012.
*   http://www.packtpub.com/cool-projects-with-opencv/book
*****************************************************************************/

////////////////////////////////////////////////////////////////////
// Standard includes:
#include <iostream>
#include <sstream>

////////////////////////////////////////////////////////////////////
// File includes:
#include "MarkerDetector.hpp"
#include "Marker.hpp"
#include "TinyLA.hpp"

MarkerDetector::MarkerDetector(CameraCalibration calibration)
    : m_minContourLengthAllowed(100)
    , markerSize(100,100)
{
    cv::Mat(3,3, CV_32F, const_cast<float*>(&calibration.getIntrinsic().data[0])).copyTo(camMatrix);
    cv::Mat(4,1, CV_32F, const_cast<float*>(&calibration.getDistorsion().data[0])).copyTo(distCoeff);

    bool centerOrigin = true;
    if (centerOrigin)
    {
        m_markerCorners3d.push_back(cv::Point3f(-0.5f,-0.5f,0));
        m_markerCorners3d.push_back(cv::Point3f(+0.5f,-0.5f,0));
        m_markerCorners3d.push_back(cv::Point3f(+0.5f,+0.5f,0));
        m_markerCorners3d.push_back(cv::Point3f(-0.5f,+0.5f,0));
    }
    else
    {
        m_markerCorners3d.push_back(cv::Point3f(0,0,0));
        m_markerCorners3d.push_back(cv::Point3f(1,0,0));
        m_markerCorners3d.push_back(cv::Point3f(1,1,0));
        m_markerCorners3d.push_back(cv::Point3f(0,1,0));
    }

    m_markerCorners2d.push_back(cv::Point2f(0,0));
    m_markerCorners2d.push_back(cv::Point2f(markerSize.width-1,0));
    m_markerCorners2d.push_back(cv::Point2f(markerSize.width-1,markerSize.height-1));
    m_markerCorners2d.push_back(cv::Point2f(0,markerSize.height-1));
}

void MarkerDetector::processFrame(const cv::Mat& frame)
{
    m_markers.clear();
    findMarkers(frame, m_markers);

}

void MarkerDetector::getTransformations(std::vector<Transformation>& t) const
{
    t.clear();
    for (size_t i=0; i<m_markers.size(); i++)
    {
        t.push_back(m_markers[i].transformation);
    }
}

bool MarkerDetector::findMarkers(const cv::Mat& frame, std::vector<Marker>& detectedMarkers)
{
    // Convert the image to grayscale
    prepareImage(frame, m_grayscaleImage);

    // Make it binary
    performThreshold(m_grayscaleImage, m_thresholdImg);

    // Detect contours
    findContours(m_thresholdImg, m_contours, m_grayscaleImage.cols / 5);

    // Find closed contours that can be approximated with 4 points
    findCandidates(m_contours, detectedMarkers);

    // Find is them are markers
    recognizeMarkers(m_grayscaleImage, detectedMarkers);

    // Calculate their poses
    estimatePosition(detectedMarkers);

    //sort by id
    std::sort(detectedMarkers.begin(), detectedMarkers.end());
    return false;
}

void MarkerDetector::prepareImage(const cv::Mat& frame, cv::Mat& grayscale) const
{
    // Convert to grayscale
    if (frame.channels() == 4)
        cv::cvtColor(frame, grayscale, CV_BGRA2GRAY);
    else if (frame.channels() == 3)
        cv::cvtColor(frame, grayscale, CV_BGR2GRAY);
    else if (frame.channels() == 1)
        frame.copyTo(grayscale);
}

void MarkerDetector::performThreshold(const cv::Mat& grayscale, cv::Mat& thresholdImg) const
{
    cv::threshold(grayscale, thresholdImg, 127, 255, cv::THRESH_BINARY_INV);

    /*
    cv::adaptiveThreshold(grayscale,   // Input image
    thresholdImg,// Result binary image
    255,         //
    cv::ADAPTIVE_THRESH_GAUSSIAN_C, //
    cv::THRESH_BINARY_INV, //
    7, //
    7  //
    );
    */

#ifdef SHOW_DEBUG_IMAGES
    cv::showAndSave("Threshold image", thresholdImg);
#endif
}

void MarkerDetector::findContours(cv::Mat& thresholdImg, ContoursVector& contours, int minContourPointsAllowed) const
{
    ContoursVector allContours;
    cv::findContours(thresholdImg, allContours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);

    contours.clear();
    for (size_t i=0; i<allContours.size(); i++)
    {
        int contourSize = allContours[i].size();
        if (contourSize > minContourPointsAllowed)
        {
            contours.push_back(allContours[i]);
        }
    }

#ifdef SHOW_DEBUG_IMAGES
    {
        cv::Mat contoursImage(thresholdImg.size(), CV_8UC1);
        contoursImage = cv::Scalar(0);
        cv::drawContours(contoursImage, contours, -1, cv::Scalar(255), 2, CV_AA);
        cv::showAndSave("Contours", contoursImage);
    }
#endif
}

void MarkerDetector::findCandidates
(
    const ContoursVector& contours,
    std::vector<Marker>& detectedMarkers
)
{
    std::vector<cv::Point>  approxCurve;
    std::vector<Marker>     possibleMarkers;

    // For each contour, analyze if it is a parallelepiped likely to be the marker
    for (size_t i=0; i<contours.size(); i++)
    {
        // Approximate to a polygon
        double eps = contours[i].size() * 0.05;
        cv::approxPolyDP(contours[i], approxCurve, eps, true);

        // We interested only in polygons that contains only four points
        if (approxCurve.size() != 4)
            continue;

        // And they have to be convex
        if (!cv::isContourConvex(approxCurve))
            continue;

        // Ensure that the distance between consecutive points is large enough
        float minDist = std::numeric_limits<float>::max();

        for (int i = 0; i < 4; i++)
        {
            cv::Point side = approxCurve[i] - approxCurve[(i+1)%4];
            float squaredSideLength = side.dot(side);
            minDist = std::min(minDist, squaredSideLength);
        }

        // Check that distance is not very small
        if (minDist < m_minContourLengthAllowed)
            continue;

        // All tests are passed. Save marker candidate:
        Marker m;

        for (int i = 0; i<4; i++)
            m.points.push_back( cv::Point2f(approxCurve[i].x,approxCurve[i].y) );

        // Sort the points in anti-clockwise order
        // Trace a line between the first and second point.
        // If the third point is at the right side, then the points are anti-clockwise
        cv::Point v1 = m.points[1] - m.points[0];
        cv::Point v2 = m.points[2] - m.points[0];

        double o = (v1.x * v2.y) - (v1.y * v2.x);

        if (o < 0.0)		 //if the third point is in the left side, then sort in anti-clockwise order
            std::swap(m.points[1], m.points[3]);

        possibleMarkers.push_back(m);
    }

    // Remove these elements which corners are too close to each other.
    // First detect candidates for removal:
    std::vector< std::pair<int,int> > tooNearCandidates;
    for (size_t i=0;i<possibleMarkers.size();i++)
    {
        const Marker& m1 = possibleMarkers[i];

        //calculate the average distance of each corner to the nearest corner of the other marker candidate
        for (size_t j=i+1;j<possibleMarkers.size();j++)
        {
            const Marker& m2 = possibleMarkers[j];

            float distSquared = 0;

            for (int c = 0; c < 4; c++)
            {
                cv::Point v = m1.points[c] - m2.points[c];
                distSquared += v.dot(v);
            }

            distSquared /= 4;

            if (distSquared < 100)
            {
                tooNearCandidates.push_back(std::pair<int,int>(i,j));
            }
        }
    }

    // Mark for removal the element of the pair with smaller perimeter
    std::vector<bool> removalMask (possibleMarkers.size(), false);

    for (size_t i=0; i<tooNearCandidates.size(); i++)
    {
        float p1 = perimeter(possibleMarkers[tooNearCandidates[i].first ].points);
        float p2 = perimeter(possibleMarkers[tooNearCandidates[i].second].points);

        size_t removalIndex;
        if (p1 > p2)
            removalIndex = tooNearCandidates[i].second;
        else
            removalIndex = tooNearCandidates[i].first;

        removalMask[removalIndex] = true;
    }

    // Return candidates
    detectedMarkers.clear();
    for (size_t i=0;i<possibleMarkers.size();i++)
    {
        if (!removalMask[i])
            detectedMarkers.push_back(possibleMarkers[i]);
    }
}

void MarkerDetector::recognizeMarkers(const cv::Mat& grayscale, std::vector<Marker>& detectedMarkers)
{
    std::vector<Marker> goodMarkers;

    // Identify the markers
    for (size_t i=0;i<detectedMarkers.size();i++)
    {
        Marker& marker = detectedMarkers[i];

        // Find the perspective transformation that brings current marker to rectangular form
        cv::Mat markerTransform = cv::getPerspectiveTransform(marker.points, m_markerCorners2d);

        // Transform image to get a canonical marker image
        cv::warpPerspective(grayscale, canonicalMarkerImage,  markerTransform, markerSize);

#ifdef SHOW_DEBUG_IMAGES
        {
            cv::Mat markerImage = grayscale.clone();
            marker.drawContour(markerImage);
            cv::Mat markerSubImage = markerImage(cv::boundingRect(marker.points));

            cv::showAndSave("Source marker" + ToString(i),           markerSubImage);
            cv::showAndSave("Marker " + ToString(i) + " after warp", canonicalMarkerImage);
        }
#endif

        int nRotations;
        int id = Marker::getMarkerId(canonicalMarkerImage, nRotations);
        if (id !=- 1)
        {
            marker.id = id;
            //sort the points so that they are always in the same order no matter the camera orientation
            std::rotate(marker.points.begin(), marker.points.begin() + 4 - nRotations, marker.points.end());

            goodMarkers.push_back(marker);
        }
    }

    // Refine marker corners using sub pixel accuracy
    if (goodMarkers.size() > 0)
    {
        std::vector<cv::Point2f> preciseCorners(4 * goodMarkers.size());

        for (size_t i=0; i<goodMarkers.size(); i++)
        {
            const Marker& marker = goodMarkers[i];

            for (int c = 0; c <4; c++)
            {
                preciseCorners[i*4 + c] = marker.points[c];
            }
        }

        cv::TermCriteria termCriteria = cv::TermCriteria(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 30, 0.01);
        cv::cornerSubPix(grayscale, preciseCorners, cvSize(5,5), cvSize(-1,-1), termCriteria);

        // Copy refined corners position back to markers
        for (size_t i=0; i<goodMarkers.size(); i++)
        {
            Marker& marker = goodMarkers[i];

            for (int c=0;c<4;c++)
            {
                marker.points[c] = preciseCorners[i*4 + c];
            }
        }
    }

#if SHOW_DEBUG_IMAGES
    {
        cv::Mat markerCornersMat(grayscale.size(), grayscale.type());
        markerCornersMat = cv::Scalar(0);

        for (size_t i=0; i<goodMarkers.size(); i++)
        {
            goodMarkers[i].drawContour(markerCornersMat, cv::Scalar(255));
        }

        cv::showAndSave("Markers refined edges", grayscale * 0.5 + markerCornersMat);
    }
#endif

    detectedMarkers = goodMarkers;
}

void MarkerDetector::estimatePosition(std::vector<Marker>& detectedMarkers)
{
    for (size_t i=0; i<detectedMarkers.size(); i++)
    {
        Marker& m = detectedMarkers[i];

        cv::Mat Rvec;
        cv::Mat_<float> Tvec;
        cv::Mat raux,taux;
        cv::solvePnP(m_markerCorners3d, m.points, camMatrix, distCoeff,raux,taux);
        raux.convertTo(Rvec,CV_32F);
        taux.convertTo(Tvec ,CV_32F);

        cv::Mat_<float> rotMat(3,3);
        cv::Rodrigues(Rvec, rotMat);

        // Copy to transformation matrix
        for (int col=0; col<3; col++)
        {
            for (int row=0; row<3; row++)
            {
                m.transformation.r().mat[row][col] = rotMat(row,col); // Copy rotation component
            }
            m.transformation.t().data[col] = Tvec(col); // Copy translation component
        }

        // Since solvePnP finds camera location, w.r.t to marker pose, to get marker pose w.r.t to the camera we invert it.
        m.transformation = m.transformation.getInverted();
    }
}
<commit_msg>Fix compilation error under linux<commit_after>/*****************************************************************************
*   MarkerDetector.cpp
*   Example_MarkerBasedAR
******************************************************************************
*   by Khvedchenia Ievgen, 5th Dec 2012
*   http://computer-vision-talks.com
******************************************************************************
*   Ch2 of the book "Mastering OpenCV with Practical Computer Vision Projects"
*   Copyright Packt Publishing 2012.
*   http://www.packtpub.com/cool-projects-with-opencv/book
*****************************************************************************/

////////////////////////////////////////////////////////////////////
// Standard includes:
#include <iostream>
#include <sstream>
#include <limits>

////////////////////////////////////////////////////////////////////
// File includes:
#include "MarkerDetector.hpp"
#include "Marker.hpp"
#include "TinyLA.hpp"

MarkerDetector::MarkerDetector(CameraCalibration calibration)
    : m_minContourLengthAllowed(100)
    , markerSize(100,100)
{
    cv::Mat(3,3, CV_32F, const_cast<float*>(&calibration.getIntrinsic().data[0])).copyTo(camMatrix);
    cv::Mat(4,1, CV_32F, const_cast<float*>(&calibration.getDistorsion().data[0])).copyTo(distCoeff);

    bool centerOrigin = true;
    if (centerOrigin)
    {
        m_markerCorners3d.push_back(cv::Point3f(-0.5f,-0.5f,0));
        m_markerCorners3d.push_back(cv::Point3f(+0.5f,-0.5f,0));
        m_markerCorners3d.push_back(cv::Point3f(+0.5f,+0.5f,0));
        m_markerCorners3d.push_back(cv::Point3f(-0.5f,+0.5f,0));
    }
    else
    {
        m_markerCorners3d.push_back(cv::Point3f(0,0,0));
        m_markerCorners3d.push_back(cv::Point3f(1,0,0));
        m_markerCorners3d.push_back(cv::Point3f(1,1,0));
        m_markerCorners3d.push_back(cv::Point3f(0,1,0));
    }

    m_markerCorners2d.push_back(cv::Point2f(0,0));
    m_markerCorners2d.push_back(cv::Point2f(markerSize.width-1,0));
    m_markerCorners2d.push_back(cv::Point2f(markerSize.width-1,markerSize.height-1));
    m_markerCorners2d.push_back(cv::Point2f(0,markerSize.height-1));
}

void MarkerDetector::processFrame(const cv::Mat& frame)
{
    m_markers.clear();
    findMarkers(frame, m_markers);

}

void MarkerDetector::getTransformations(std::vector<Transformation>& t) const
{
    t.clear();
    for (size_t i=0; i<m_markers.size(); i++)
    {
        t.push_back(m_markers[i].transformation);
    }
}

bool MarkerDetector::findMarkers(const cv::Mat& frame, std::vector<Marker>& detectedMarkers)
{
    // Convert the image to grayscale
    prepareImage(frame, m_grayscaleImage);

    // Make it binary
    performThreshold(m_grayscaleImage, m_thresholdImg);

    // Detect contours
    findContours(m_thresholdImg, m_contours, m_grayscaleImage.cols / 5);

    // Find closed contours that can be approximated with 4 points
    findCandidates(m_contours, detectedMarkers);

    // Find is them are markers
    recognizeMarkers(m_grayscaleImage, detectedMarkers);

    // Calculate their poses
    estimatePosition(detectedMarkers);

    //sort by id
    std::sort(detectedMarkers.begin(), detectedMarkers.end());
    return false;
}

void MarkerDetector::prepareImage(const cv::Mat& frame, cv::Mat& grayscale) const
{
    // Convert to grayscale
    if (frame.channels() == 4)
        cv::cvtColor(frame, grayscale, cv::COLOR_BGRA2GRAY);
    else if (frame.channels() == 3)
        cv::cvtColor(frame, grayscale, cv::COLOR_BGR2GRAY);
    else if (frame.channels() == 1)
        frame.copyTo(grayscale);
}

void MarkerDetector::performThreshold(const cv::Mat& grayscale, cv::Mat& thresholdImg) const
{
    cv::threshold(grayscale, thresholdImg, 127, 255, cv::THRESH_BINARY_INV);

    /*
    cv::adaptiveThreshold(grayscale,   // Input image
    thresholdImg,// Result binary image
    255,         //
    cv::ADAPTIVE_THRESH_GAUSSIAN_C, //
    cv::THRESH_BINARY_INV, //
    7, //
    7  //
    );
    */

#ifdef SHOW_DEBUG_IMAGES
    cv::showAndSave("Threshold image", thresholdImg);
#endif
}

void MarkerDetector::findContours(cv::Mat& thresholdImg, ContoursVector& contours, int minContourPointsAllowed) const
{
    ContoursVector allContours;
    cv::findContours(thresholdImg, allContours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);

    contours.clear();
    for (size_t i=0; i<allContours.size(); i++)
    {
        int contourSize = allContours[i].size();
        if (contourSize > minContourPointsAllowed)
        {
            contours.push_back(allContours[i]);
        }
    }

#ifdef SHOW_DEBUG_IMAGES
    {
        cv::Mat contoursImage(thresholdImg.size(), CV_8UC1);
        contoursImage = cv::Scalar(0);
        cv::drawContours(contoursImage, contours, -1, cv::Scalar(255), 2, CV_AA);
        cv::showAndSave("Contours", contoursImage);
    }
#endif
}

void MarkerDetector::findCandidates
(
    const ContoursVector& contours,
    std::vector<Marker>& detectedMarkers
)
{
    std::vector<cv::Point>  approxCurve;
    std::vector<Marker>     possibleMarkers;

    // For each contour, analyze if it is a parallelepiped likely to be the marker
    for (size_t i=0; i<contours.size(); i++)
    {
        // Approximate to a polygon
        double eps = contours[i].size() * 0.05;
        cv::approxPolyDP(contours[i], approxCurve, eps, true);

        // We interested only in polygons that contains only four points
        if (approxCurve.size() != 4)
            continue;

        // And they have to be convex
        if (!cv::isContourConvex(approxCurve))
            continue;

        // Ensure that the distance between consecutive points is large enough
        float minDist = std::numeric_limits<float>::max();

        for (int i = 0; i < 4; i++)
        {
            cv::Point side = approxCurve[i] - approxCurve[(i+1)%4];
            float squaredSideLength = side.dot(side);
            minDist = std::min(minDist, squaredSideLength);
        }

        // Check that distance is not very small
        if (minDist < m_minContourLengthAllowed)
            continue;

        // All tests are passed. Save marker candidate:
        Marker m;

        for (int i = 0; i<4; i++)
            m.points.push_back( cv::Point2f(approxCurve[i].x,approxCurve[i].y) );

        // Sort the points in anti-clockwise order
        // Trace a line between the first and second point.
        // If the third point is at the right side, then the points are anti-clockwise
        cv::Point v1 = m.points[1] - m.points[0];
        cv::Point v2 = m.points[2] - m.points[0];

        double o = (v1.x * v2.y) - (v1.y * v2.x);

        if (o < 0.0)		 //if the third point is in the left side, then sort in anti-clockwise order
            std::swap(m.points[1], m.points[3]);

        possibleMarkers.push_back(m);
    }

    // Remove these elements which corners are too close to each other.
    // First detect candidates for removal:
    std::vector< std::pair<int,int> > tooNearCandidates;
    for (size_t i=0;i<possibleMarkers.size();i++)
    {
        const Marker& m1 = possibleMarkers[i];

        //calculate the average distance of each corner to the nearest corner of the other marker candidate
        for (size_t j=i+1;j<possibleMarkers.size();j++)
        {
            const Marker& m2 = possibleMarkers[j];

            float distSquared = 0;

            for (int c = 0; c < 4; c++)
            {
                cv::Point v = m1.points[c] - m2.points[c];
                distSquared += v.dot(v);
            }

            distSquared /= 4;

            if (distSquared < 100)
            {
                tooNearCandidates.push_back(std::pair<int,int>(i,j));
            }
        }
    }

    // Mark for removal the element of the pair with smaller perimeter
    std::vector<bool> removalMask (possibleMarkers.size(), false);

    for (size_t i=0; i<tooNearCandidates.size(); i++)
    {
        float p1 = perimeter(possibleMarkers[tooNearCandidates[i].first ].points);
        float p2 = perimeter(possibleMarkers[tooNearCandidates[i].second].points);

        size_t removalIndex;
        if (p1 > p2)
            removalIndex = tooNearCandidates[i].second;
        else
            removalIndex = tooNearCandidates[i].first;

        removalMask[removalIndex] = true;
    }

    // Return candidates
    detectedMarkers.clear();
    for (size_t i=0;i<possibleMarkers.size();i++)
    {
        if (!removalMask[i])
            detectedMarkers.push_back(possibleMarkers[i]);
    }
}

void MarkerDetector::recognizeMarkers(const cv::Mat& grayscale, std::vector<Marker>& detectedMarkers)
{
    std::vector<Marker> goodMarkers;

    // Identify the markers
    for (size_t i=0;i<detectedMarkers.size();i++)
    {
        Marker& marker = detectedMarkers[i];

        // Find the perspective transformation that brings current marker to rectangular form
        cv::Mat markerTransform = cv::getPerspectiveTransform(marker.points, m_markerCorners2d);

        // Transform image to get a canonical marker image
        cv::warpPerspective(grayscale, canonicalMarkerImage,  markerTransform, markerSize);

#ifdef SHOW_DEBUG_IMAGES
        {
            cv::Mat markerImage = grayscale.clone();
            marker.drawContour(markerImage);
            cv::Mat markerSubImage = markerImage(cv::boundingRect(marker.points));

            cv::showAndSave("Source marker" + ToString(i),           markerSubImage);
            cv::showAndSave("Marker " + ToString(i) + " after warp", canonicalMarkerImage);
        }
#endif

        int nRotations;
        int id = Marker::getMarkerId(canonicalMarkerImage, nRotations);
        if (id !=- 1)
        {
            marker.id = id;
            //sort the points so that they are always in the same order no matter the camera orientation
            std::rotate(marker.points.begin(), marker.points.begin() + 4 - nRotations, marker.points.end());

            goodMarkers.push_back(marker);
        }
    }

    // Refine marker corners using sub pixel accuracy
    if (goodMarkers.size() > 0)
    {
        std::vector<cv::Point2f> preciseCorners(4 * goodMarkers.size());

        for (size_t i=0; i<goodMarkers.size(); i++)
        {
            const Marker& marker = goodMarkers[i];

            for (int c = 0; c <4; c++)
            {
                preciseCorners[i*4 + c] = marker.points[c];
            }
        }

        cv::TermCriteria termCriteria = cv::TermCriteria(cv::TermCriteria::MAX_ITER | cv::TermCriteria::EPS, 30, 0.01);
        cv::cornerSubPix(grayscale, preciseCorners, cvSize(5,5), cvSize(-1,-1), termCriteria);

        // Copy refined corners position back to markers
        for (size_t i=0; i<goodMarkers.size(); i++)
        {
            Marker& marker = goodMarkers[i];

            for (int c=0;c<4;c++)
            {
                marker.points[c] = preciseCorners[i*4 + c];
            }
        }
    }

#if SHOW_DEBUG_IMAGES
    {
        cv::Mat markerCornersMat(grayscale.size(), grayscale.type());
        markerCornersMat = cv::Scalar(0);

        for (size_t i=0; i<goodMarkers.size(); i++)
        {
            goodMarkers[i].drawContour(markerCornersMat, cv::Scalar(255));
        }

        cv::showAndSave("Markers refined edges", grayscale * 0.5 + markerCornersMat);
    }
#endif

    detectedMarkers = goodMarkers;
}

void MarkerDetector::estimatePosition(std::vector<Marker>& detectedMarkers)
{
    for (size_t i=0; i<detectedMarkers.size(); i++)
    {
        Marker& m = detectedMarkers[i];

        cv::Mat Rvec;
        cv::Mat_<float> Tvec;
        cv::Mat raux,taux;
        cv::solvePnP(m_markerCorners3d, m.points, camMatrix, distCoeff,raux,taux);
        raux.convertTo(Rvec,CV_32F);
        taux.convertTo(Tvec ,CV_32F);

        cv::Mat_<float> rotMat(3,3);
        cv::Rodrigues(Rvec, rotMat);

        // Copy to transformation matrix
        for (int col=0; col<3; col++)
        {
            for (int row=0; row<3; row++)
            {
                m.transformation.r().mat[row][col] = rotMat(row,col); // Copy rotation component
            }
            m.transformation.t().data[col] = Tvec(col); // Copy translation component
        }

        // Since solvePnP finds camera location, w.r.t to marker pose, to get marker pose w.r.t to the camera we invert it.
        m.transformation = m.transformation.getInverted();
    }
}
<|endoftext|>


<commit_before>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * OpenSceneGraph Public License for more details.
*/

//
// OpenFlight loader for OpenSceneGraph
//
//  Copyright (C) 2005-2007  Brede Johansen
//

#include <osg/Notify>
#include <osg/TexEnv>
#include <osg/Texture2D>
#include <osg/StateSet>
#include <osg/GL>

#include <osgDB/FileNameUtils>
#include <osgDB/FileUtils>
#include <osgDB/Registry>

#include "AttrData.h"
#include "DataInputStream.h"

using namespace osg;
using namespace osgDB;
using namespace flt;

class ReaderWriterATTR : public osgDB::ReaderWriter
{
    public:

        virtual const char* className() const { return "ATTR Image Attribute Reader/Writer"; }

        virtual bool acceptsExtension(const std::string& extension) const
        {
            return equalCaseInsensitive(extension,"attr");
        }

        virtual ReadResult readObject(const std::string& fileName, const ReaderWriter::Options*) const;
};

ReaderWriter::ReadResult ReaderWriterATTR::readObject(const std::string& file, const ReaderWriter::Options* options) const
{
    using std::ios;

    std::string ext = osgDB::getLowerCaseFileExtension(file);
    if (!acceptsExtension(ext)) return ReadResult::FILE_NOT_HANDLED;

    std::string fileName = osgDB::findDataFile( file, options );
    if (fileName.empty()) return ReadResult::FILE_NOT_FOUND;

    std::ifstream fin;
    fin.imbue(std::locale::classic());
    fin.open(fileName.c_str(), std::ios::in | std::ios::binary);

    if ( fin.fail())
        return ReadResult::ERROR_IN_READING_FILE;

    flt::DataInputStream in(fin.rdbuf());

    AttrData* attr = new AttrData;

    try
    {
        attr->texels_u  = in.readInt32();
        attr->textel_v  = in.readInt32();
        attr->direction_u  = in.readInt32();
        attr->direction_v  = in.readInt32();
        attr->x_up  = in.readInt32();
        attr->y_up  = in.readInt32();
        attr->fileFormat  = in.readInt32();
        attr->minFilterMode  = in.readInt32();
        attr->magFilterMode  = in.readInt32();
        attr->wrapMode  = in.readInt32(AttrData::WRAP_REPEAT);

        attr->wrapMode_u  = in.readInt32();
        if (attr->wrapMode_u == AttrData::WRAP_NONE)
            attr->wrapMode_u = attr->wrapMode;

        attr->wrapMode_v  = in.readInt32();
        if (attr->wrapMode_v == AttrData::WRAP_NONE)
            attr->wrapMode_v = attr->wrapMode;

        attr->modifyFlag = in.readInt32();
        attr->pivot_x  = in.readInt32();
        attr->pivot_y  = in.readInt32();

        // v11 ends here
//      if (in.eof() || (_flt_version <= 11)) return true;
#if 1
        attr->texEnvMode = in.readInt32(AttrData::TEXENV_MODULATE);
        attr->intensityAsAlpha = in.readInt32();
        in.forward(4*8);
        attr->size_u = in.readFloat64();
        attr->size_v = in.readFloat64();
        attr->originCode = in.readInt32();
        attr->kernelVersion = in.readInt32();
        attr->intFormat = in.readInt32();
        attr->extFormat = in.readInt32();
        attr->useMips = in.readInt32();
        for (int n=0; n<8; n++)
            attr->of_mips[n] = in.readFloat32();
        attr->useLodScale = in.readInt32();
        attr->lod0 = in.readFloat32();
        attr->scale0 = in.readFloat32();
        attr->lod1 = in.readFloat32();
        attr->scale1 = in.readFloat32();
        attr->lod2 = in.readFloat32();
        attr->scale2 = in.readFloat32();
        attr->lod3 = in.readFloat32();
        attr->scale3 = in.readFloat32();
        attr->lod4 = in.readFloat32();
        attr->scale4 = in.readFloat32();
        attr->lod5 = in.readFloat32();
        attr->scale5 = in.readFloat32();
        attr->lod6 = in.readFloat32();
        attr->scale6 = in.readFloat32();
        attr->lod7 = in.readFloat32();
        attr->scale7 = in.readFloat32();
        attr->clamp = in.readFloat32();
        attr->magFilterAlpha = in.readInt32();
        attr->magFilterColor = in.readInt32();
        in.forward(4);
        in.forward(4*8);
        attr->lambertMeridian = in.readFloat64();
        attr->lambertUpperLat = in.readFloat64();
        attr->lambertlowerLat = in.readFloat64();
        in.forward(8);
        in.forward(4*5);
        attr->useDetail = in.readInt32(  );
        attr->txDetail_j = in.readInt32();
        attr->txDetail_k = in.readInt32();
        attr->txDetail_m = in.readInt32();
        attr->txDetail_n = in.readInt32();
        attr->txDetail_s = in.readInt32( );
        attr->useTile = in.readInt32();
        attr->txTile_ll_u= in.readFloat32();
        attr->txTile_ll_v = in.readFloat32();
        attr->txTile_ur_u = in.readFloat32();
        attr->txTile_ur_v = in.readFloat32();
        attr->projection = in.readInt32();
        attr->earthModel = in.readInt32();
        in.forward(4);
        attr->utmZone = in.readInt32();
        attr->imageOrigin = in.readInt32();
        attr->geoUnits = in.readInt32();
        in.forward(4);
        in.forward(4);
        attr->hemisphere = in.readInt32();
        in.forward(4);
        in.forward(4);
        in.forward(149*4);
        attr->comments = in.readString(512);

        // v12 ends here
//      if (in.eof() || (_flt_version <= 12)) return true;

        in.forward(13*4);
        attr->attrVersion = in.readInt32();
        attr->controlPoints = in.readInt32();
        in.forward(4);
#endif
    }
    catch(...)
    {
        if (!fin.eof())
        {
            throw;
        }
    }

    fin.close();

    return attr;
}

// now register with Registry to instantiate the above
// reader/writer.
REGISTER_OSGPLUGIN(attr, ReaderWriterATTR)

<commit_msg>From Andreas Ekstrand and Lars Nilsson, fix for reading Texture Attribute file <commit_after>/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
 *
 * This library is open source and may be redistributed and/or modified under
 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
 * (at your option) any later version.  The full license is in LICENSE file
 * included with this distribution, and on the openscenegraph.org website.
 *
 * This library is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * OpenSceneGraph Public License for more details.
*/

//
// OpenFlight loader for OpenSceneGraph
//
//  Copyright (C) 2005-2007  Brede Johansen
//

#include <osg/Notify>
#include <osg/TexEnv>
#include <osg/Texture2D>
#include <osg/StateSet>
#include <osg/GL>

#include <osgDB/FileNameUtils>
#include <osgDB/FileUtils>
#include <osgDB/Registry>

#include "AttrData.h"
#include "DataInputStream.h"

using namespace osg;
using namespace osgDB;
using namespace flt;

class ReaderWriterATTR : public osgDB::ReaderWriter
{
    public:

        virtual const char* className() const { return "ATTR Image Attribute Reader/Writer"; }

        virtual bool acceptsExtension(const std::string& extension) const
        {
            return equalCaseInsensitive(extension,"attr");
        }

        virtual ReadResult readObject(const std::string& fileName, const ReaderWriter::Options*) const;
};

ReaderWriter::ReadResult ReaderWriterATTR::readObject(const std::string& file, const ReaderWriter::Options* options) const
{
    using std::ios;

    std::string ext = osgDB::getLowerCaseFileExtension(file);
    if (!acceptsExtension(ext)) return ReadResult::FILE_NOT_HANDLED;

    std::string fileName = osgDB::findDataFile( file, options );
    if (fileName.empty()) return ReadResult::FILE_NOT_FOUND;

    std::ifstream fin;
    fin.imbue(std::locale::classic());
    fin.open(fileName.c_str(), std::ios::in | std::ios::binary);

    if ( fin.fail())
        return ReadResult::ERROR_IN_READING_FILE;

    flt::DataInputStream in(fin.rdbuf());

    AttrData* attr = new AttrData;

    try
    {
        attr->texels_u  = in.readInt32();
        attr->textel_v  = in.readInt32();
        attr->direction_u  = in.readInt32();
        attr->direction_v  = in.readInt32();
        attr->x_up  = in.readInt32();
        attr->y_up  = in.readInt32();
        attr->fileFormat  = in.readInt32();
        attr->minFilterMode  = in.readInt32();
        attr->magFilterMode  = in.readInt32();
        attr->wrapMode  = in.readInt32(AttrData::WRAP_REPEAT);

        attr->wrapMode_u  = in.readInt32();
        if (attr->wrapMode_u == AttrData::WRAP_NONE)
            attr->wrapMode_u = attr->wrapMode;

        attr->wrapMode_v  = in.readInt32();
        if (attr->wrapMode_v == AttrData::WRAP_NONE)
            attr->wrapMode_v = attr->wrapMode;

        attr->modifyFlag = in.readInt32();
        attr->pivot_x  = in.readInt32();
        attr->pivot_y  = in.readInt32();

        // v11 ends here
//      if (in.eof() || (_flt_version <= 11)) return true;
#if 1
        attr->texEnvMode = in.readInt32(AttrData::TEXENV_MODULATE);
        attr->intensityAsAlpha = in.readInt32();
        in.forward(4*8);
        in.forward(4);
        attr->size_u = in.readFloat64();
        attr->size_v = in.readFloat64();
        attr->originCode = in.readInt32();
        attr->kernelVersion = in.readInt32();
        attr->intFormat = in.readInt32();
        attr->extFormat = in.readInt32();
        attr->useMips = in.readInt32();
        for (int n=0; n<8; n++)
            attr->of_mips[n] = in.readFloat32();
        attr->useLodScale = in.readInt32();
        attr->lod0 = in.readFloat32();
        attr->scale0 = in.readFloat32();
        attr->lod1 = in.readFloat32();
        attr->scale1 = in.readFloat32();
        attr->lod2 = in.readFloat32();
        attr->scale2 = in.readFloat32();
        attr->lod3 = in.readFloat32();
        attr->scale3 = in.readFloat32();
        attr->lod4 = in.readFloat32();
        attr->scale4 = in.readFloat32();
        attr->lod5 = in.readFloat32();
        attr->scale5 = in.readFloat32();
        attr->lod6 = in.readFloat32();
        attr->scale6 = in.readFloat32();
        attr->lod7 = in.readFloat32();
        attr->scale7 = in.readFloat32();
        attr->clamp = in.readFloat32();
        attr->magFilterAlpha = in.readInt32();
        attr->magFilterColor = in.readInt32();
        in.forward(4);
        in.forward(4*8);
        attr->lambertMeridian = in.readFloat64();
        attr->lambertUpperLat = in.readFloat64();
        attr->lambertlowerLat = in.readFloat64();
        in.forward(8);
        in.forward(4*5);
        attr->useDetail = in.readInt32(  );
        attr->txDetail_j = in.readInt32();
        attr->txDetail_k = in.readInt32();
        attr->txDetail_m = in.readInt32();
        attr->txDetail_n = in.readInt32();
        attr->txDetail_s = in.readInt32( );
        attr->useTile = in.readInt32();
        attr->txTile_ll_u= in.readFloat32();
        attr->txTile_ll_v = in.readFloat32();
        attr->txTile_ur_u = in.readFloat32();
        attr->txTile_ur_v = in.readFloat32();
        attr->projection = in.readInt32();
        attr->earthModel = in.readInt32();
        in.forward(4);
        attr->utmZone = in.readInt32();
        attr->imageOrigin = in.readInt32();
        attr->geoUnits = in.readInt32();
        in.forward(4);
        in.forward(4);
        attr->hemisphere = in.readInt32();
        in.forward(4);
        in.forward(4);
        in.forward(149*4);
        attr->comments = in.readString(512);

        // v12 ends here
//      if (in.eof() || (_flt_version <= 12)) return true;

        in.forward(14*4);
        attr->attrVersion = in.readInt32();
        attr->controlPoints = in.readInt32();
        in.forward(4);
#endif
    }
    catch(...)
    {
        if (!fin.eof())
        {
            throw;
        }
    }

    fin.close();

    return attr;
}

// now register with Registry to instantiate the above
// reader/writer.
REGISTER_OSGPLUGIN(attr, ReaderWriterATTR)

<|endoftext|>


<commit_before>/** Copyright 2008, 2009, 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018 Roland Olbricht et al.
 *
 * This file is part of Overpass_API.
 *
 * Overpass_API is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * Overpass_API is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with Overpass_API.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "clone_database.h"
#include "../core/datatypes.h"
#include "../core/settings.h"
#include "../../template_db/block_backend.h"
#include "../../template_db/file_blocks.h"
#include "../../template_db/random_file.h"

template< class TIndex >
void clone_bin_file(const File_Properties& src_file_prop, const File_Properties& dest_file_prop,
		    Transaction& transaction, std::string dest_db_dir, const Clone_Settings& clone_settings)

{
  try
  {
    if (src_file_prop.get_block_size() * src_file_prop.get_compression_factor()
        != dest_file_prop.get_block_size() * dest_file_prop.get_compression_factor())
    {
      std::cout<<"Block sizes of source and destination format are incompatible.\n";
      return;
    }
    uint32 block_size = src_file_prop.get_block_size() * src_file_prop.get_compression_factor();

    File_Blocks_Index< TIndex >& src_idx =
        *dynamic_cast< File_Blocks_Index< TIndex >* >(transaction.data_index(&src_file_prop));
    File_Blocks< TIndex, typename std::set< TIndex >::const_iterator, Default_Range_Iterator< TIndex > >
	src_file(&src_idx);

    File_Blocks_Index< TIndex > dest_idx(dest_file_prop, true, false, dest_db_dir, "",
        clone_settings.compression_method);
    File_Blocks< TIndex, typename std::set< TIndex >::const_iterator, Default_Range_Iterator< TIndex > >
	dest_file(&dest_idx);

    typename File_Blocks< TIndex, typename std::set< TIndex >::const_iterator,
        Default_Range_Iterator< TIndex > >::Flat_Iterator
	src_it = src_file.flat_begin();

    uint32 max_keysize = 0;
    while (!src_it.is_end())
    {
      if (max_keysize > 0)
      {
        uint64* buf = src_file.read_block(src_it, false);
        dest_file.insert_block(
            dest_file.write_end(), buf, std::min(max_keysize, block_size),
            src_it.block_it->max_keysize, src_it.block_it->index);
        max_keysize = std::max(max_keysize, block_size) - block_size;
      }
      else
      {
        uint64* buf = src_file.read_block(src_it);
        dest_file.insert_block(dest_file.write_end(), buf, src_it.block_it->max_keysize);
        if (src_it.block_it->max_keysize > block_size)
          max_keysize = src_it.block_it->max_keysize - block_size;
      }
      ++src_it;
    }
  }
  catch (File_Error e)
  {
    std::cout<<e.origin<<' '<<e.error_number<<' '<<strerror(e.error_number)<<' '<<e.filename<<'\n';
  }
}

template< typename Key, typename TIndex >
void clone_map_file(const File_Properties& file_prop, Transaction& transaction, std::string dest_db_dir, Clone_Settings clone_settings)
{
  try
  {
    Random_File_Index& src_idx = *transaction.random_index(&file_prop);
    Random_File< Key, TIndex > src_file(&src_idx);

    Random_File_Index dest_idx(file_prop, true, false, dest_db_dir, "", clone_settings.map_compression_method);
    Random_File< Key, TIndex > dest_file(&dest_idx);

    for (std::vector< uint32 >::size_type i = 0; i < src_idx.get_blocks().size(); ++i)
    {
      if (src_idx.get_blocks()[i].pos != src_idx.npos)
      {
	for (uint32 j = 0; j < src_idx.get_block_size()*src_idx.get_compression_factor()/TIndex::max_size_of(); ++j)
	{
	  TIndex val =
	      src_file.get(i*(src_idx.get_block_size()*src_idx.get_compression_factor()/TIndex::max_size_of()) + j);
	  if (!(val == TIndex(uint32(0))))
	    dest_file.put(i*(src_idx.get_block_size()*src_idx.get_compression_factor()/TIndex::max_size_of()) + j, val);
	}
      }
    }
  }
  catch (File_Error e)
  {
    std::cout<<e.origin<<' '<<e.error_number<<' '<<strerror(e.error_number)<<' '<<e.filename<<'\n';
  }
}

void clone_database(Transaction& transaction, const std::string& dest_db_dir, const Clone_Settings& clone_settings)
{
  clone_bin_file< Uint32_Index >(*osm_base_settings().NODES, *osm_base_settings().NODES,
				 transaction, dest_db_dir, clone_settings);
  clone_map_file< Node_Skeleton::Id_Type, Uint32_Index >(*osm_base_settings().NODES, transaction, dest_db_dir, clone_settings);
  clone_bin_file< Tag_Index_Local >(*osm_base_settings().NODE_TAGS_LOCAL, *osm_base_settings().NODE_TAGS_LOCAL,
				    transaction, dest_db_dir, clone_settings);
  clone_bin_file< Tag_Index_Global >(*osm_base_settings().NODE_TAGS_GLOBAL, *osm_base_settings().NODE_TAGS_GLOBAL,
				     transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint32_Index >(*osm_base_settings().NODE_KEYS, *osm_base_settings().NODE_KEYS,
				 transaction, dest_db_dir, clone_settings);

  clone_bin_file< Uint31_Index >(*osm_base_settings().WAYS, *osm_base_settings().WAYS,
				 transaction, dest_db_dir, clone_settings);
  clone_map_file< Way_Skeleton::Id_Type, Uint31_Index >(*osm_base_settings().WAYS, transaction, dest_db_dir, clone_settings);
  clone_bin_file< Tag_Index_Local >(*osm_base_settings().WAY_TAGS_LOCAL, *osm_base_settings().WAY_TAGS_LOCAL,
				    transaction, dest_db_dir, clone_settings);
  clone_bin_file< Tag_Index_Global >(*osm_base_settings().WAY_TAGS_GLOBAL, *osm_base_settings().WAY_TAGS_GLOBAL,
				     transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint32_Index >(*osm_base_settings().WAY_KEYS, *osm_base_settings().WAY_KEYS,
				 transaction, dest_db_dir, clone_settings);

  clone_bin_file< Uint31_Index >(*osm_base_settings().RELATIONS, *osm_base_settings().RELATIONS,
				 transaction, dest_db_dir, clone_settings);
  clone_map_file< Relation_Skeleton::Id_Type, Uint31_Index >(
      *osm_base_settings().RELATIONS, transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint32_Index >(*osm_base_settings().RELATION_ROLES, *osm_base_settings().RELATION_ROLES,
				 transaction, dest_db_dir, clone_settings);
  clone_bin_file< Tag_Index_Local >(
      *osm_base_settings().RELATION_TAGS_LOCAL, *osm_base_settings().RELATION_TAGS_LOCAL,
      transaction, dest_db_dir, clone_settings);
  clone_bin_file< Tag_Index_Global >(
      *osm_base_settings().RELATION_TAGS_GLOBAL, *osm_base_settings().RELATION_TAGS_GLOBAL,
      transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint32_Index >(*osm_base_settings().RELATION_KEYS, *osm_base_settings().RELATION_KEYS,
				 transaction, dest_db_dir, clone_settings);

  clone_bin_file< Uint31_Index >(*meta_settings().NODES_META, *meta_settings().NODES_META,
				 transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint31_Index >(*meta_settings().WAYS_META, *meta_settings().WAYS_META,
				 transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint31_Index >(*meta_settings().RELATIONS_META, *meta_settings().RELATIONS_META,
				 transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint32_Index >(*meta_settings().USER_DATA, *meta_settings().USER_DATA,
				 transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint32_Index >(*meta_settings().USER_INDICES, *meta_settings().USER_INDICES,
				 transaction, dest_db_dir, clone_settings);

  {
    clone_bin_file< Uint31_Index >(*attic_settings().NODES, *attic_settings().NODES,
                                   transaction, dest_db_dir, clone_settings);
    clone_map_file< Node_Skeleton::Id_Type, Uint31_Index >(*attic_settings().NODES, transaction, dest_db_dir, clone_settings);
    clone_bin_file< Uint31_Index >(*attic_settings().NODES_UNDELETED, *attic_settings().NODES_UNDELETED,
                                   transaction, dest_db_dir, clone_settings);
    clone_bin_file< Node::Id_Type >(*attic_settings().NODE_IDX_LIST, *attic_settings().NODE_IDX_LIST,
                                    transaction, dest_db_dir, clone_settings);
    clone_bin_file< Tag_Index_Local >(*attic_settings().NODE_TAGS_LOCAL, *attic_settings().NODE_TAGS_LOCAL,
                                      transaction, dest_db_dir, clone_settings);
    clone_bin_file< Tag_Index_Global >(*attic_settings().NODE_TAGS_GLOBAL, *attic_settings().NODE_TAGS_GLOBAL,
                                       transaction, dest_db_dir, clone_settings);
    clone_bin_file< Uint31_Index >(*attic_settings().NODES_META, *attic_settings().NODES_META,
                                   transaction, dest_db_dir, clone_settings);
    clone_bin_file< Timestamp >(*attic_settings().NODE_CHANGELOG, *attic_settings().NODE_CHANGELOG,
                                transaction, dest_db_dir, clone_settings);

    clone_bin_file< Uint31_Index >(*attic_settings().WAYS, *attic_settings().WAYS,
                                   transaction, dest_db_dir, clone_settings);
    clone_map_file< Way_Skeleton::Id_Type, Uint31_Index >(*attic_settings().WAYS, transaction, dest_db_dir, clone_settings);
    clone_bin_file< Uint31_Index >(*attic_settings().WAYS_UNDELETED, *attic_settings().WAYS_UNDELETED,
                                   transaction, dest_db_dir, clone_settings);
    clone_bin_file< Way::Id_Type >(*attic_settings().WAY_IDX_LIST, *attic_settings().WAY_IDX_LIST,
                                   transaction, dest_db_dir, clone_settings);
    clone_bin_file< Tag_Index_Local >(*attic_settings().WAY_TAGS_LOCAL, *attic_settings().WAY_TAGS_LOCAL,
                                      transaction, dest_db_dir, clone_settings);
    clone_bin_file< Tag_Index_Global >(*attic_settings().WAY_TAGS_GLOBAL, *attic_settings().WAY_TAGS_GLOBAL,
                                       transaction, dest_db_dir, clone_settings);
    clone_bin_file< Uint31_Index >(*attic_settings().WAYS_META, *attic_settings().WAYS_META,
                                   transaction, dest_db_dir, clone_settings);
    clone_bin_file< Timestamp >(*attic_settings().WAY_CHANGELOG, *attic_settings().WAY_CHANGELOG,
                                transaction, dest_db_dir, clone_settings);

    clone_bin_file< Uint31_Index >(*attic_settings().RELATIONS, *attic_settings().RELATIONS,
                                   transaction, dest_db_dir, clone_settings);
    clone_map_file< Relation_Skeleton::Id_Type, Uint31_Index >(*attic_settings().RELATIONS, transaction, dest_db_dir, clone_settings);
    clone_bin_file< Uint31_Index >(*attic_settings().RELATIONS_UNDELETED, *attic_settings().RELATIONS_UNDELETED,
                                   transaction, dest_db_dir, clone_settings);
    clone_bin_file< Relation::Id_Type >(*attic_settings().RELATION_IDX_LIST, *attic_settings().RELATION_IDX_LIST,
                                        transaction, dest_db_dir, clone_settings);
    clone_bin_file< Tag_Index_Local >(
        *attic_settings().RELATION_TAGS_LOCAL, *attic_settings().RELATION_TAGS_LOCAL,
        transaction, dest_db_dir, clone_settings);
    clone_bin_file< Tag_Index_Global >(
        *attic_settings().RELATION_TAGS_GLOBAL, *attic_settings().RELATION_TAGS_GLOBAL,
        transaction, dest_db_dir, clone_settings);
    clone_bin_file< Uint31_Index >(*attic_settings().RELATIONS_META, *attic_settings().RELATIONS_META,
                                   transaction, dest_db_dir, clone_settings);
    clone_bin_file< Timestamp >(*attic_settings().RELATION_CHANGELOG, *attic_settings().RELATION_CHANGELOG,
                                transaction, dest_db_dir, clone_settings);
  }
}
<commit_msg>Fixed a subtle bug in the cloning mechanism.<commit_after>/** Copyright 2008, 2009, 2010, 2011, 2012, 2013, 2014, 2015, 2016, 2017, 2018 Roland Olbricht et al.
 *
 * This file is part of Overpass_API.
 *
 * Overpass_API is free software: you can redistribute it and/or modify
 * it under the terms of the GNU Affero General Public License as
 * published by the Free Software Foundation, either version 3 of the
 * License, or (at your option) any later version.
 *
 * Overpass_API is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU Affero General Public License
 * along with Overpass_API.  If not, see <http://www.gnu.org/licenses/>.
 */

#include "clone_database.h"
#include "../core/datatypes.h"
#include "../core/settings.h"
#include "../../template_db/block_backend.h"
#include "../../template_db/file_blocks.h"
#include "../../template_db/random_file.h"

template< class TIndex >
void clone_bin_file(const File_Properties& src_file_prop, const File_Properties& dest_file_prop,
		    Transaction& transaction, std::string dest_db_dir, const Clone_Settings& clone_settings)

{
  try
  {
    if (src_file_prop.get_block_size() * src_file_prop.get_compression_factor()
        != dest_file_prop.get_block_size() * dest_file_prop.get_compression_factor())
    {
      std::cout<<"Block sizes of source and destination format are incompatible.\n";
      return;
    }
    uint32 block_size = src_file_prop.get_block_size() * src_file_prop.get_compression_factor();

    File_Blocks_Index< TIndex >& src_idx =
        *dynamic_cast< File_Blocks_Index< TIndex >* >(transaction.data_index(&src_file_prop));
    File_Blocks< TIndex, typename std::set< TIndex >::const_iterator, Default_Range_Iterator< TIndex > >
	src_file(&src_idx);

    File_Blocks_Index< TIndex > dest_idx(dest_file_prop, true, false, dest_db_dir, "",
        clone_settings.compression_method);
    File_Blocks< TIndex, typename std::set< TIndex >::const_iterator, Default_Range_Iterator< TIndex > >
	dest_file(&dest_idx);

    typename File_Blocks< TIndex, typename std::set< TIndex >::const_iterator,
        Default_Range_Iterator< TIndex > >::Flat_Iterator
	src_it = src_file.flat_begin();

    uint32 excess_bytes = 0;
    while (!src_it.is_end())
    {
      if (excess_bytes > 0)
      {
        uint64* buf = src_file.read_block(src_it, false);
        dest_file.insert_block(
            dest_file.write_end(), buf, std::min(excess_bytes, block_size),
            src_it.block_it->max_keysize, src_it.block_it->index);
        excess_bytes = std::max(excess_bytes, block_size) - block_size;
      }
      else
      {
        uint64* buf = src_file.read_block(src_it);
        dest_file.insert_block(dest_file.write_end(), buf, src_it.block_it->max_keysize);
        if (src_it.block_it->max_keysize + 4 > block_size)
          excess_bytes = src_it.block_it->max_keysize + 4 - block_size;
      }
      ++src_it;
    }
  }
  catch (File_Error e)
  {
    std::cout<<e.origin<<' '<<e.error_number<<' '<<strerror(e.error_number)<<' '<<e.filename<<'\n';
  }
}

template< typename Key, typename TIndex >
void clone_map_file(const File_Properties& file_prop, Transaction& transaction, std::string dest_db_dir, Clone_Settings clone_settings)
{
  try
  {
    Random_File_Index& src_idx = *transaction.random_index(&file_prop);
    Random_File< Key, TIndex > src_file(&src_idx);

    Random_File_Index dest_idx(file_prop, true, false, dest_db_dir, "", clone_settings.map_compression_method);
    Random_File< Key, TIndex > dest_file(&dest_idx);

    for (std::vector< uint32 >::size_type i = 0; i < src_idx.get_blocks().size(); ++i)
    {
      if (src_idx.get_blocks()[i].pos != src_idx.npos)
      {
	for (uint32 j = 0; j < src_idx.get_block_size()*src_idx.get_compression_factor()/TIndex::max_size_of(); ++j)
	{
	  TIndex val =
	      src_file.get(i*(src_idx.get_block_size()*src_idx.get_compression_factor()/TIndex::max_size_of()) + j);
	  if (!(val == TIndex(uint32(0))))
	    dest_file.put(i*(src_idx.get_block_size()*src_idx.get_compression_factor()/TIndex::max_size_of()) + j, val);
	}
      }
    }
  }
  catch (File_Error e)
  {
    std::cout<<e.origin<<' '<<e.error_number<<' '<<strerror(e.error_number)<<' '<<e.filename<<'\n';
  }
}

void clone_database(Transaction& transaction, const std::string& dest_db_dir, const Clone_Settings& clone_settings)
{
  clone_bin_file< Uint32_Index >(*osm_base_settings().NODES, *osm_base_settings().NODES,
				 transaction, dest_db_dir, clone_settings);
  clone_map_file< Node_Skeleton::Id_Type, Uint32_Index >(*osm_base_settings().NODES, transaction, dest_db_dir, clone_settings);
  clone_bin_file< Tag_Index_Local >(*osm_base_settings().NODE_TAGS_LOCAL, *osm_base_settings().NODE_TAGS_LOCAL,
				    transaction, dest_db_dir, clone_settings);
  clone_bin_file< Tag_Index_Global >(*osm_base_settings().NODE_TAGS_GLOBAL, *osm_base_settings().NODE_TAGS_GLOBAL,
				     transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint32_Index >(*osm_base_settings().NODE_KEYS, *osm_base_settings().NODE_KEYS,
				 transaction, dest_db_dir, clone_settings);

  clone_bin_file< Uint31_Index >(*osm_base_settings().WAYS, *osm_base_settings().WAYS,
				 transaction, dest_db_dir, clone_settings);
  clone_map_file< Way_Skeleton::Id_Type, Uint31_Index >(*osm_base_settings().WAYS, transaction, dest_db_dir, clone_settings);
  clone_bin_file< Tag_Index_Local >(*osm_base_settings().WAY_TAGS_LOCAL, *osm_base_settings().WAY_TAGS_LOCAL,
				    transaction, dest_db_dir, clone_settings);
  clone_bin_file< Tag_Index_Global >(*osm_base_settings().WAY_TAGS_GLOBAL, *osm_base_settings().WAY_TAGS_GLOBAL,
				     transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint32_Index >(*osm_base_settings().WAY_KEYS, *osm_base_settings().WAY_KEYS,
				 transaction, dest_db_dir, clone_settings);

  clone_bin_file< Uint31_Index >(*osm_base_settings().RELATIONS, *osm_base_settings().RELATIONS,
				 transaction, dest_db_dir, clone_settings);
  clone_map_file< Relation_Skeleton::Id_Type, Uint31_Index >(
      *osm_base_settings().RELATIONS, transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint32_Index >(*osm_base_settings().RELATION_ROLES, *osm_base_settings().RELATION_ROLES,
				 transaction, dest_db_dir, clone_settings);
  clone_bin_file< Tag_Index_Local >(
      *osm_base_settings().RELATION_TAGS_LOCAL, *osm_base_settings().RELATION_TAGS_LOCAL,
      transaction, dest_db_dir, clone_settings);
  clone_bin_file< Tag_Index_Global >(
      *osm_base_settings().RELATION_TAGS_GLOBAL, *osm_base_settings().RELATION_TAGS_GLOBAL,
      transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint32_Index >(*osm_base_settings().RELATION_KEYS, *osm_base_settings().RELATION_KEYS,
				 transaction, dest_db_dir, clone_settings);

  clone_bin_file< Uint31_Index >(*meta_settings().NODES_META, *meta_settings().NODES_META,
				 transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint31_Index >(*meta_settings().WAYS_META, *meta_settings().WAYS_META,
				 transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint31_Index >(*meta_settings().RELATIONS_META, *meta_settings().RELATIONS_META,
				 transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint32_Index >(*meta_settings().USER_DATA, *meta_settings().USER_DATA,
				 transaction, dest_db_dir, clone_settings);
  clone_bin_file< Uint32_Index >(*meta_settings().USER_INDICES, *meta_settings().USER_INDICES,
				 transaction, dest_db_dir, clone_settings);

  {
    clone_bin_file< Uint31_Index >(*attic_settings().NODES, *attic_settings().NODES,
                                   transaction, dest_db_dir, clone_settings);
    clone_map_file< Node_Skeleton::Id_Type, Uint31_Index >(*attic_settings().NODES, transaction, dest_db_dir, clone_settings);
    clone_bin_file< Uint31_Index >(*attic_settings().NODES_UNDELETED, *attic_settings().NODES_UNDELETED,
                                   transaction, dest_db_dir, clone_settings);
    clone_bin_file< Node::Id_Type >(*attic_settings().NODE_IDX_LIST, *attic_settings().NODE_IDX_LIST,
                                    transaction, dest_db_dir, clone_settings);
    clone_bin_file< Tag_Index_Local >(*attic_settings().NODE_TAGS_LOCAL, *attic_settings().NODE_TAGS_LOCAL,
                                      transaction, dest_db_dir, clone_settings);
    clone_bin_file< Tag_Index_Global >(*attic_settings().NODE_TAGS_GLOBAL, *attic_settings().NODE_TAGS_GLOBAL,
                                       transaction, dest_db_dir, clone_settings);
    clone_bin_file< Uint31_Index >(*attic_settings().NODES_META, *attic_settings().NODES_META,
                                   transaction, dest_db_dir, clone_settings);
    clone_bin_file< Timestamp >(*attic_settings().NODE_CHANGELOG, *attic_settings().NODE_CHANGELOG,
                                transaction, dest_db_dir, clone_settings);

    clone_bin_file< Uint31_Index >(*attic_settings().WAYS, *attic_settings().WAYS,
                                   transaction, dest_db_dir, clone_settings);
    clone_map_file< Way_Skeleton::Id_Type, Uint31_Index >(*attic_settings().WAYS, transaction, dest_db_dir, clone_settings);
    clone_bin_file< Uint31_Index >(*attic_settings().WAYS_UNDELETED, *attic_settings().WAYS_UNDELETED,
                                   transaction, dest_db_dir, clone_settings);
    clone_bin_file< Way::Id_Type >(*attic_settings().WAY_IDX_LIST, *attic_settings().WAY_IDX_LIST,
                                   transaction, dest_db_dir, clone_settings);
    clone_bin_file< Tag_Index_Local >(*attic_settings().WAY_TAGS_LOCAL, *attic_settings().WAY_TAGS_LOCAL,
                                      transaction, dest_db_dir, clone_settings);
    clone_bin_file< Tag_Index_Global >(*attic_settings().WAY_TAGS_GLOBAL, *attic_settings().WAY_TAGS_GLOBAL,
                                       transaction, dest_db_dir, clone_settings);
    clone_bin_file< Uint31_Index >(*attic_settings().WAYS_META, *attic_settings().WAYS_META,
                                   transaction, dest_db_dir, clone_settings);
    clone_bin_file< Timestamp >(*attic_settings().WAY_CHANGELOG, *attic_settings().WAY_CHANGELOG,
                                transaction, dest_db_dir, clone_settings);

    clone_bin_file< Uint31_Index >(*attic_settings().RELATIONS, *attic_settings().RELATIONS,
                                   transaction, dest_db_dir, clone_settings);
    clone_map_file< Relation_Skeleton::Id_Type, Uint31_Index >(*attic_settings().RELATIONS, transaction, dest_db_dir, clone_settings);
    clone_bin_file< Uint31_Index >(*attic_settings().RELATIONS_UNDELETED, *attic_settings().RELATIONS_UNDELETED,
                                   transaction, dest_db_dir, clone_settings);
    clone_bin_file< Relation::Id_Type >(*attic_settings().RELATION_IDX_LIST, *attic_settings().RELATION_IDX_LIST,
                                        transaction, dest_db_dir, clone_settings);
    clone_bin_file< Tag_Index_Local >(
        *attic_settings().RELATION_TAGS_LOCAL, *attic_settings().RELATION_TAGS_LOCAL,
        transaction, dest_db_dir, clone_settings);
    clone_bin_file< Tag_Index_Global >(
        *attic_settings().RELATION_TAGS_GLOBAL, *attic_settings().RELATION_TAGS_GLOBAL,
        transaction, dest_db_dir, clone_settings);
    clone_bin_file< Uint31_Index >(*attic_settings().RELATIONS_META, *attic_settings().RELATIONS_META,
                                   transaction, dest_db_dir, clone_settings);
    clone_bin_file< Timestamp >(*attic_settings().RELATION_CHANGELOG, *attic_settings().RELATION_CHANGELOG,
                                transaction, dest_db_dir, clone_settings);
  }
}
<|endoftext|>


<commit_before>/*
  This file is part of Ingen.
  Copyright 2007-2012 David Robillard <http://drobilla.net/>

  Ingen is free software: you can redistribute it and/or modify it under the
  terms of the GNU Affero General Public License as published by the Free
  Software Foundation, either version 3 of the License, or any later version.

  Ingen is distributed in the hope that it will be useful, but WITHOUT ANY
  WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
  A PARTICULAR PURPOSE.  See the GNU Affero General Public License for details.

  You should have received a copy of the GNU Affero General Public License
  along with Ingen.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "ingen/Node.hpp"
#include "ingen/URIs.hpp"
#include "ingen/client/ObjectModel.hpp"

namespace Ingen {
namespace Client {

ObjectModel::ObjectModel(URIs& uris, const Raul::Path& path)
	: Node(uris, path)
	, _path(path)
	, _symbol((path == "/") ? "root" : path.symbol())
{
}

ObjectModel::ObjectModel(const ObjectModel& copy)
	: Node(copy)
	, _parent(copy._parent)
	, _path(copy._path)
	, _symbol(copy._symbol)
{
}

ObjectModel::~ObjectModel()
{
}

bool
ObjectModel::is_a(const Raul::URI& type) const
{
	return has_property(_uris.rdf_type, _uris.forge.alloc_uri(type));
}

void
ObjectModel::on_property(const Raul::URI& uri, const Atom& value)
{
	_signal_property.emit(uri, value);
}

void
ObjectModel::on_property_removed(const Raul::URI& uri, const Atom& value)
{
	_signal_property_removed.emit(uri, value);
}

const Atom&
ObjectModel::get_property(const Raul::URI& key) const
{
	static const Atom null_atom;
	Resource::Properties::const_iterator i = properties().find(key);
	return (i != properties().end()) ? i->second : null_atom;
}

bool
ObjectModel::polyphonic() const
{
	const Atom& polyphonic = get_property(_uris.ingen_polyphonic);
	return (polyphonic.is_valid() && polyphonic.get<int32_t>());
}

/** Merge the data of `o` with self, as much as possible.
 *
 * This will merge the two models, but with any conflict take the value in
 * `o` as correct.  The paths of the two models MUST be equal.
 */
void
ObjectModel::set(SPtr<ObjectModel> o)
{
	assert(_path == o->path());
	if (o->_parent)
		_parent = o->_parent;

	for (auto v : o->properties()) {
		Resource::set_property(v.first, v.second);
		_signal_property.emit(v.first, v.second);
	}
}

void
ObjectModel::set_path(const Raul::Path& p)
{
	_path   = p;
	_symbol = Raul::Symbol(p.is_root() ? "root" : p.symbol());
	_signal_moved.emit();
}

void
ObjectModel::set_parent(SPtr<ObjectModel> p)
{
	assert(_path.is_child_of(p->path()));
	_parent = p;
}

} // namespace Client
} // namespace Ingen
<commit_msg>Fix renaming.<commit_after>/*
  This file is part of Ingen.
  Copyright 2007-2012 David Robillard <http://drobilla.net/>

  Ingen is free software: you can redistribute it and/or modify it under the
  terms of the GNU Affero General Public License as published by the Free
  Software Foundation, either version 3 of the License, or any later version.

  Ingen is distributed in the hope that it will be useful, but WITHOUT ANY
  WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR
  A PARTICULAR PURPOSE.  See the GNU Affero General Public License for details.

  You should have received a copy of the GNU Affero General Public License
  along with Ingen.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "ingen/Node.hpp"
#include "ingen/URIs.hpp"
#include "ingen/client/ObjectModel.hpp"

namespace Ingen {
namespace Client {

ObjectModel::ObjectModel(URIs& uris, const Raul::Path& path)
	: Node(uris, path)
	, _path(path)
	, _symbol((path == "/") ? "root" : path.symbol())
{
}

ObjectModel::ObjectModel(const ObjectModel& copy)
	: Node(copy)
	, _parent(copy._parent)
	, _path(copy._path)
	, _symbol(copy._symbol)
{
}

ObjectModel::~ObjectModel()
{
}

bool
ObjectModel::is_a(const Raul::URI& type) const
{
	return has_property(_uris.rdf_type, _uris.forge.alloc_uri(type));
}

void
ObjectModel::on_property(const Raul::URI& uri, const Atom& value)
{
	_signal_property.emit(uri, value);
}

void
ObjectModel::on_property_removed(const Raul::URI& uri, const Atom& value)
{
	_signal_property_removed.emit(uri, value);
}

const Atom&
ObjectModel::get_property(const Raul::URI& key) const
{
	static const Atom null_atom;
	Resource::Properties::const_iterator i = properties().find(key);
	return (i != properties().end()) ? i->second : null_atom;
}

bool
ObjectModel::polyphonic() const
{
	const Atom& polyphonic = get_property(_uris.ingen_polyphonic);
	return (polyphonic.is_valid() && polyphonic.get<int32_t>());
}

/** Merge the data of `o` with self, as much as possible.
 *
 * This will merge the two models, but with any conflict take the value in
 * `o` as correct.  The paths of the two models MUST be equal.
 */
void
ObjectModel::set(SPtr<ObjectModel> o)
{
	assert(_path == o->path());
	if (o->_parent)
		_parent = o->_parent;

	for (auto v : o->properties()) {
		Resource::set_property(v.first, v.second);
		_signal_property.emit(v.first, v.second);
	}
}

void
ObjectModel::set_path(const Raul::Path& p)
{
	_path   = p;
	_symbol = Raul::Symbol(p.is_root() ? "root" : p.symbol());
	set_uri(Node::path_to_uri(p));
	_signal_moved.emit();
}

void
ObjectModel::set_parent(SPtr<ObjectModel> p)
{
	assert(_path.is_child_of(p->path()));
	_parent = p;
}

} // namespace Client
} // namespace Ingen
<|endoftext|>


<commit_before>/*
 *
 *    Copyright (c) 2021 Project CHIP Authors
 *
 *    Licensed under the Apache License, Version 2.0 (the "License");
 *    you may not use this file except in compliance with the License.
 *    You may obtain a copy of the License at
 *
 *        http://www.apache.org/licenses/LICENSE-2.0
 *
 *    Unless required by applicable law or agreed to in writing, software
 *    distributed under the License is distributed on an "AS IS" BASIS,
 *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *    See the License for the specific language governing permissions and
 *    limitations under the License.
 */

#include "ChipDeviceScanner.h"

#if CHIP_DEVICE_CONFIG_ENABLE_CHIPOBLE

#include "BluezObjectList.h"
#include "MainLoop.h"
#include "Types.h"

#include <errno.h>
#include <lib/support/logging/CHIPLogging.h>
#include <pthread.h>

namespace chip {
namespace DeviceLayer {
namespace Internal {
namespace {

struct GObjectUnref
{
    template <typename T>
    void operator()(T * value)
    {
        g_object_unref(value);
    }
};

using GCancellableUniquePtr       = std::unique_ptr<GCancellable, GObjectUnref>;
using GDBusObjectManagerUniquePtr = std::unique_ptr<GDBusObjectManager, GObjectUnref>;

/// Retrieve CHIP device identification info from the device advertising data
bool BluezGetChipDeviceInfo(BluezDevice1 & aDevice, chip::Ble::ChipBLEDeviceIdentificationInfo & aDeviceInfo)
{
    GVariant * serviceData = bluez_device1_get_service_data(&aDevice);
    VerifyOrReturnError(serviceData != nullptr, false);

    GVariant * dataValue = g_variant_lookup_value(serviceData, CHIP_BLE_UUID_SERVICE_STRING, nullptr);
    VerifyOrReturnError(dataValue != nullptr, false);

    size_t dataLen         = 0;
    const void * dataBytes = g_variant_get_fixed_array(dataValue, &dataLen, sizeof(uint8_t));
    VerifyOrReturnError(dataBytes != nullptr && dataLen >= sizeof(aDeviceInfo), false);

    memcpy(&aDeviceInfo, dataBytes, sizeof(aDeviceInfo));
    return true;
}

} // namespace

ChipDeviceScanner::ChipDeviceScanner(GDBusObjectManager * manager, BluezAdapter1 * adapter, GCancellable * cancellable,
                                     ChipDeviceScannerDelegate * delegate) :
    mManager(manager),
    mAdapter(adapter), mCancellable(cancellable), mDelegate(delegate)
{
    g_object_ref(mAdapter);
    g_object_ref(mCancellable);
    g_object_ref(mManager);
}

ChipDeviceScanner::~ChipDeviceScanner()
{
    StopScan();

    // In case the timeout timer is still active
    chip::DeviceLayer::SystemLayer().CancelTimer(TimerExpiredCallback, this);

    g_object_unref(mManager);
    g_object_unref(mCancellable);
    g_object_unref(mAdapter);

    mManager     = nullptr;
    mAdapter     = nullptr;
    mCancellable = nullptr;
    mDelegate    = nullptr;
}

std::unique_ptr<ChipDeviceScanner> ChipDeviceScanner::Create(BluezAdapter1 * adapter, ChipDeviceScannerDelegate * delegate)
{
    GError * error = nullptr;

    GCancellableUniquePtr cancellable(g_cancellable_new(), GObjectUnref());

    if (!cancellable)
    {
        return std::unique_ptr<ChipDeviceScanner>();
    }

    GDBusObjectManagerUniquePtr manager(
        g_dbus_object_manager_client_new_for_bus_sync(G_BUS_TYPE_SYSTEM, G_DBUS_OBJECT_MANAGER_CLIENT_FLAGS_NONE, BLUEZ_INTERFACE,
                                                      "/", bluez_object_manager_client_get_proxy_type,
                                                      nullptr /* unused user data in the Proxy Type Func */,
                                                      nullptr /*destroy notify */, cancellable.get(), &error),
        GObjectUnref());
    if (!manager)
    {
        ChipLogError(Ble, "Failed to get DBUS object manager for device scanning: %s", error->message);
        g_error_free(error);
        return std::unique_ptr<ChipDeviceScanner>();
    }

    return std::make_unique<ChipDeviceScanner>(manager.get(), adapter, cancellable.get(), delegate);
}

CHIP_ERROR ChipDeviceScanner::StartScan(unsigned timeoutMs)
{
    ReturnErrorCodeIf(mIsScanning, CHIP_ERROR_INCORRECT_STATE);

    ReturnErrorOnFailure(MainLoop::Instance().EnsureStarted());

    mIsScanning = true; // optimistic, to allow all callbacks to check this
    if (!MainLoop::Instance().Schedule(MainLoopStartScan, this))
    {
        ChipLogError(Ble, "Failed to schedule BLE scan start.");
        mIsScanning = false;
        return CHIP_ERROR_INTERNAL;
    }

    CHIP_ERROR err = chip::DeviceLayer::SystemLayer().StartTimer(timeoutMs, TimerExpiredCallback, static_cast<void *>(this));

    if (err != CHIP_NO_ERROR)
    {
        ChipLogError(Ble, "Failed to schedule scan timeout.");
        StopScan();
        return err;
    }

    return CHIP_NO_ERROR;
}

void ChipDeviceScanner::TimerExpiredCallback(chip::System::Layer * layer, void * appState)
{
    static_cast<ChipDeviceScanner *>(appState)->StopScan();
}

CHIP_ERROR ChipDeviceScanner::StopScan()
{
    ReturnErrorCodeIf(!mIsScanning, CHIP_NO_ERROR);
    ReturnErrorCodeIf(mIsStopping, CHIP_NO_ERROR);
    mIsStopping = true;
    g_cancellable_cancel(mCancellable); // in case we are currently running a scan

    if (mObjectAddedSignal)
    {
        g_signal_handler_disconnect(mManager, mObjectAddedSignal);
        mObjectAddedSignal = 0;
    }

    if (mInterfaceChangedSignal)
    {
        g_signal_handler_disconnect(mManager, mInterfaceChangedSignal);
        mInterfaceChangedSignal = 0;
    }

    if (!MainLoop::Instance().ScheduleAndWait(MainLoopStopScan, this))
    {
        ChipLogError(Ble, "Failed to schedule BLE scan stop.");
        return CHIP_ERROR_INTERNAL;
    }

    return CHIP_NO_ERROR;
}

int ChipDeviceScanner::MainLoopStopScan(ChipDeviceScanner * self)
{
    GError * error = nullptr;

    if (!bluez_adapter1_call_stop_discovery_sync(self->mAdapter, nullptr /* not cancellable */, &error))
    {
        ChipLogError(Ble, "Failed to stop discovery %s", error->message);
        g_error_free(error);
    }
    ChipDeviceScannerDelegate * delegate = self->mDelegate;
    self->mIsScanning                    = false;

    // callback is explicitly allowed to delete the scanner (hence no more
    // references to 'self' here)
    delegate->OnScanComplete();

    return 0;
}

void ChipDeviceScanner::SignalObjectAdded(GDBusObjectManager * manager, GDBusObject * object, ChipDeviceScanner * self)
{
    self->ReportDevice(bluez_object_get_device1(BLUEZ_OBJECT(object)));
}

void ChipDeviceScanner::SignalInterfaceChanged(GDBusObjectManagerClient * manager, GDBusObjectProxy * object,
                                               GDBusProxy * aInterface, GVariant * aChangedProperties,
                                               const gchar * const * aInvalidatedProps, ChipDeviceScanner * self)
{
    self->ReportDevice(bluez_object_get_device1(BLUEZ_OBJECT(object)));
}

void ChipDeviceScanner::ReportDevice(BluezDevice1 * device)
{
    if (device == nullptr)
    {
        return;
    }

    if (strcmp(bluez_device1_get_adapter(device), g_dbus_proxy_get_object_path(G_DBUS_PROXY(mAdapter))) != 0)
    {
        return;
    }

    chip::Ble::ChipBLEDeviceIdentificationInfo deviceInfo;

    if (!BluezGetChipDeviceInfo(*device, deviceInfo))
    {
        ChipLogDetail(Ble, "Device %s does not look like a CHIP device.", bluez_device1_get_address(device));
        return;
    }

    mDelegate->OnDeviceScanned(device, deviceInfo);
}

void ChipDeviceScanner::RemoveDevice(BluezDevice1 * device)
{
    if (device == nullptr)
    {
        return;
    }

    if (strcmp(bluez_device1_get_adapter(device), g_dbus_proxy_get_object_path(G_DBUS_PROXY(mAdapter))) != 0)
    {
        return;
    }

    chip::Ble::ChipBLEDeviceIdentificationInfo deviceInfo;

    if (!BluezGetChipDeviceInfo(*device, deviceInfo))
    {
        return;
    }

    const auto devicePath = g_dbus_proxy_get_object_path(G_DBUS_PROXY(device));
    GError * error        = nullptr;

    if (!bluez_adapter1_call_remove_device_sync(mAdapter, devicePath, nullptr, &error))
    {
        ChipLogDetail(Ble, "Failed to remove device %s: %s", devicePath, error->message);
        g_error_free(error);
    }
}

int ChipDeviceScanner::MainLoopStartScan(ChipDeviceScanner * self)
{
    GError * error = nullptr;

    self->mObjectAddedSignal = g_signal_connect(self->mManager, "object-added", G_CALLBACK(SignalObjectAdded), self);
    self->mInterfaceChangedSignal =
        g_signal_connect(self->mManager, "interface-proxy-properties-changed", G_CALLBACK(SignalInterfaceChanged), self);

    ChipLogProgress(Ble, "BLE removing known devices.");
    for (BluezObject & object : BluezObjectList(self->mManager))
    {
        self->RemoveDevice(bluez_object_get_device1(&object));
    }

    /* Search for matter/chip UUID only */
    GVariantBuilder uuidsBuilder;
    GVariant * uuids;
    g_variant_builder_init(&uuidsBuilder, G_VARIANT_TYPE("as"));
    g_variant_builder_add(&uuidsBuilder, "s", CHIP_BLE_UUID_SERVICE_STRING);
    uuids = g_variant_builder_end(&uuidsBuilder);

    /* Search for LE only: Advertises */
    GVariantBuilder filterBuilder;
    GVariant * filter;

    g_variant_builder_init(&filterBuilder, G_VARIANT_TYPE("a{sv}"));
    g_variant_builder_add(&filterBuilder, "{sv}", "Transport", g_variant_new_string("le"));
    g_variant_builder_add(&filterBuilder, "{sv}", "UUIDs", uuids);
    filter = g_variant_builder_end(&filterBuilder);

    if (!bluez_adapter1_call_set_discovery_filter_sync(self->mAdapter, filter, self->mCancellable, &error))
    {
        /* Not critical: ignore if fails */
        ChipLogError(Ble, "Failed to set discovery filters: %s", error->message);
        g_error_free(error);
    }

    ChipLogProgress(Ble, "BLE initiating scan.");
    if (!bluez_adapter1_call_start_discovery_sync(self->mAdapter, self->mCancellable, &error))
    {
        ChipLogError(Ble, "Failed to start discovery: %s", error->message);
        g_error_free(error);

        self->mIsScanning = false;
        self->mDelegate->OnScanComplete();
    }

    return 0;
}

} // namespace Internal
} // namespace DeviceLayer
} // namespace chip

#endif // CHIP_DEVICE_CONFIG_ENABLE_CHIPOBLE
<commit_msg>[bluez] Remove filtering by service UUID (#9782)<commit_after>/*
 *
 *    Copyright (c) 2021 Project CHIP Authors
 *
 *    Licensed under the Apache License, Version 2.0 (the "License");
 *    you may not use this file except in compliance with the License.
 *    You may obtain a copy of the License at
 *
 *        http://www.apache.org/licenses/LICENSE-2.0
 *
 *    Unless required by applicable law or agreed to in writing, software
 *    distributed under the License is distributed on an "AS IS" BASIS,
 *    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 *    See the License for the specific language governing permissions and
 *    limitations under the License.
 */

#include "ChipDeviceScanner.h"

#if CHIP_DEVICE_CONFIG_ENABLE_CHIPOBLE

#include "BluezObjectList.h"
#include "MainLoop.h"
#include "Types.h"

#include <errno.h>
#include <lib/support/logging/CHIPLogging.h>
#include <pthread.h>

namespace chip {
namespace DeviceLayer {
namespace Internal {
namespace {

struct GObjectUnref
{
    template <typename T>
    void operator()(T * value)
    {
        g_object_unref(value);
    }
};

using GCancellableUniquePtr       = std::unique_ptr<GCancellable, GObjectUnref>;
using GDBusObjectManagerUniquePtr = std::unique_ptr<GDBusObjectManager, GObjectUnref>;

/// Retrieve CHIP device identification info from the device advertising data
bool BluezGetChipDeviceInfo(BluezDevice1 & aDevice, chip::Ble::ChipBLEDeviceIdentificationInfo & aDeviceInfo)
{
    GVariant * serviceData = bluez_device1_get_service_data(&aDevice);
    VerifyOrReturnError(serviceData != nullptr, false);

    GVariant * dataValue = g_variant_lookup_value(serviceData, CHIP_BLE_UUID_SERVICE_STRING, nullptr);
    VerifyOrReturnError(dataValue != nullptr, false);

    size_t dataLen         = 0;
    const void * dataBytes = g_variant_get_fixed_array(dataValue, &dataLen, sizeof(uint8_t));
    VerifyOrReturnError(dataBytes != nullptr && dataLen >= sizeof(aDeviceInfo), false);

    memcpy(&aDeviceInfo, dataBytes, sizeof(aDeviceInfo));
    return true;
}

} // namespace

ChipDeviceScanner::ChipDeviceScanner(GDBusObjectManager * manager, BluezAdapter1 * adapter, GCancellable * cancellable,
                                     ChipDeviceScannerDelegate * delegate) :
    mManager(manager),
    mAdapter(adapter), mCancellable(cancellable), mDelegate(delegate)
{
    g_object_ref(mAdapter);
    g_object_ref(mCancellable);
    g_object_ref(mManager);
}

ChipDeviceScanner::~ChipDeviceScanner()
{
    StopScan();

    // In case the timeout timer is still active
    chip::DeviceLayer::SystemLayer().CancelTimer(TimerExpiredCallback, this);

    g_object_unref(mManager);
    g_object_unref(mCancellable);
    g_object_unref(mAdapter);

    mManager     = nullptr;
    mAdapter     = nullptr;
    mCancellable = nullptr;
    mDelegate    = nullptr;
}

std::unique_ptr<ChipDeviceScanner> ChipDeviceScanner::Create(BluezAdapter1 * adapter, ChipDeviceScannerDelegate * delegate)
{
    GError * error = nullptr;

    GCancellableUniquePtr cancellable(g_cancellable_new(), GObjectUnref());

    if (!cancellable)
    {
        return std::unique_ptr<ChipDeviceScanner>();
    }

    GDBusObjectManagerUniquePtr manager(
        g_dbus_object_manager_client_new_for_bus_sync(G_BUS_TYPE_SYSTEM, G_DBUS_OBJECT_MANAGER_CLIENT_FLAGS_NONE, BLUEZ_INTERFACE,
                                                      "/", bluez_object_manager_client_get_proxy_type,
                                                      nullptr /* unused user data in the Proxy Type Func */,
                                                      nullptr /*destroy notify */, cancellable.get(), &error),
        GObjectUnref());
    if (!manager)
    {
        ChipLogError(Ble, "Failed to get DBUS object manager for device scanning: %s", error->message);
        g_error_free(error);
        return std::unique_ptr<ChipDeviceScanner>();
    }

    return std::make_unique<ChipDeviceScanner>(manager.get(), adapter, cancellable.get(), delegate);
}

CHIP_ERROR ChipDeviceScanner::StartScan(unsigned timeoutMs)
{
    ReturnErrorCodeIf(mIsScanning, CHIP_ERROR_INCORRECT_STATE);

    ReturnErrorOnFailure(MainLoop::Instance().EnsureStarted());

    mIsScanning = true; // optimistic, to allow all callbacks to check this
    if (!MainLoop::Instance().Schedule(MainLoopStartScan, this))
    {
        ChipLogError(Ble, "Failed to schedule BLE scan start.");
        mIsScanning = false;
        return CHIP_ERROR_INTERNAL;
    }

    CHIP_ERROR err = chip::DeviceLayer::SystemLayer().StartTimer(timeoutMs, TimerExpiredCallback, static_cast<void *>(this));

    if (err != CHIP_NO_ERROR)
    {
        ChipLogError(Ble, "Failed to schedule scan timeout.");
        StopScan();
        return err;
    }

    return CHIP_NO_ERROR;
}

void ChipDeviceScanner::TimerExpiredCallback(chip::System::Layer * layer, void * appState)
{
    static_cast<ChipDeviceScanner *>(appState)->StopScan();
}

CHIP_ERROR ChipDeviceScanner::StopScan()
{
    ReturnErrorCodeIf(!mIsScanning, CHIP_NO_ERROR);
    ReturnErrorCodeIf(mIsStopping, CHIP_NO_ERROR);
    mIsStopping = true;
    g_cancellable_cancel(mCancellable); // in case we are currently running a scan

    if (mObjectAddedSignal)
    {
        g_signal_handler_disconnect(mManager, mObjectAddedSignal);
        mObjectAddedSignal = 0;
    }

    if (mInterfaceChangedSignal)
    {
        g_signal_handler_disconnect(mManager, mInterfaceChangedSignal);
        mInterfaceChangedSignal = 0;
    }

    if (!MainLoop::Instance().ScheduleAndWait(MainLoopStopScan, this))
    {
        ChipLogError(Ble, "Failed to schedule BLE scan stop.");
        return CHIP_ERROR_INTERNAL;
    }

    return CHIP_NO_ERROR;
}

int ChipDeviceScanner::MainLoopStopScan(ChipDeviceScanner * self)
{
    GError * error = nullptr;

    if (!bluez_adapter1_call_stop_discovery_sync(self->mAdapter, nullptr /* not cancellable */, &error))
    {
        ChipLogError(Ble, "Failed to stop discovery %s", error->message);
        g_error_free(error);
    }
    ChipDeviceScannerDelegate * delegate = self->mDelegate;
    self->mIsScanning                    = false;

    // callback is explicitly allowed to delete the scanner (hence no more
    // references to 'self' here)
    delegate->OnScanComplete();

    return 0;
}

void ChipDeviceScanner::SignalObjectAdded(GDBusObjectManager * manager, GDBusObject * object, ChipDeviceScanner * self)
{
    self->ReportDevice(bluez_object_get_device1(BLUEZ_OBJECT(object)));
}

void ChipDeviceScanner::SignalInterfaceChanged(GDBusObjectManagerClient * manager, GDBusObjectProxy * object,
                                               GDBusProxy * aInterface, GVariant * aChangedProperties,
                                               const gchar * const * aInvalidatedProps, ChipDeviceScanner * self)
{
    self->ReportDevice(bluez_object_get_device1(BLUEZ_OBJECT(object)));
}

void ChipDeviceScanner::ReportDevice(BluezDevice1 * device)
{
    if (device == nullptr)
    {
        return;
    }

    if (strcmp(bluez_device1_get_adapter(device), g_dbus_proxy_get_object_path(G_DBUS_PROXY(mAdapter))) != 0)
    {
        return;
    }

    chip::Ble::ChipBLEDeviceIdentificationInfo deviceInfo;

    if (!BluezGetChipDeviceInfo(*device, deviceInfo))
    {
        ChipLogDetail(Ble, "Device %s does not look like a CHIP device.", bluez_device1_get_address(device));
        return;
    }

    mDelegate->OnDeviceScanned(device, deviceInfo);
}

void ChipDeviceScanner::RemoveDevice(BluezDevice1 * device)
{
    if (device == nullptr)
    {
        return;
    }

    if (strcmp(bluez_device1_get_adapter(device), g_dbus_proxy_get_object_path(G_DBUS_PROXY(mAdapter))) != 0)
    {
        return;
    }

    chip::Ble::ChipBLEDeviceIdentificationInfo deviceInfo;

    if (!BluezGetChipDeviceInfo(*device, deviceInfo))
    {
        return;
    }

    const auto devicePath = g_dbus_proxy_get_object_path(G_DBUS_PROXY(device));
    GError * error        = nullptr;

    if (!bluez_adapter1_call_remove_device_sync(mAdapter, devicePath, nullptr, &error))
    {
        ChipLogDetail(Ble, "Failed to remove device %s: %s", devicePath, error->message);
        g_error_free(error);
    }
}

int ChipDeviceScanner::MainLoopStartScan(ChipDeviceScanner * self)
{
    GError * error = nullptr;

    self->mObjectAddedSignal = g_signal_connect(self->mManager, "object-added", G_CALLBACK(SignalObjectAdded), self);
    self->mInterfaceChangedSignal =
        g_signal_connect(self->mManager, "interface-proxy-properties-changed", G_CALLBACK(SignalInterfaceChanged), self);

    ChipLogProgress(Ble, "BLE removing known devices.");
    for (BluezObject & object : BluezObjectList(self->mManager))
    {
        self->RemoveDevice(bluez_object_get_device1(&object));
    }

    // Search for LE only.
    // Do NOT add filtering by UUID as it is done by the following kernel function:
    // https://github.com/torvalds/linux/blob/bdb575f872175ed0ecf2638369da1cb7a6e86a14/net/bluetooth/mgmt.c#L9258.
    // The function requires that devices advertise its services' UUIDs in UUID16/32/128 fields
    // while the Matter specification requires only FLAGS (0x01) and SERVICE_DATA_16 (0x16) fields
    // in the advertisement packets.
    GVariantBuilder filterBuilder;
    g_variant_builder_init(&filterBuilder, G_VARIANT_TYPE("a{sv}"));
    g_variant_builder_add(&filterBuilder, "{sv}", "Transport", g_variant_new_string("le"));
    GVariant * filter = g_variant_builder_end(&filterBuilder);

    if (!bluez_adapter1_call_set_discovery_filter_sync(self->mAdapter, filter, self->mCancellable, &error))
    {
        // Not critical: ignore if fails
        ChipLogError(Ble, "Failed to set discovery filters: %s", error->message);
        g_error_free(error);
    }

    ChipLogProgress(Ble, "BLE initiating scan.");
    if (!bluez_adapter1_call_start_discovery_sync(self->mAdapter, self->mCancellable, &error))
    {
        ChipLogError(Ble, "Failed to start discovery: %s", error->message);
        g_error_free(error);

        self->mIsScanning = false;
        self->mDelegate->OnScanComplete();
    }

    return 0;
}

} // namespace Internal
} // namespace DeviceLayer
} // namespace chip

#endif // CHIP_DEVICE_CONFIG_ENABLE_CHIPOBLE
<|endoftext|>