CLandmark::CLandmark( const UIDLandmark& p_uID, const CDescriptor& p_matDescriptorLEFT, const CDescriptor& p_matDescriptorRIGHT, const double& p_dKeyPointSize, const cv::Point2d& p_ptUVLEFT, const cv::Point2d& p_ptUVRIGHT, const CPoint3DCAMERA& p_vecPointXYZLEFT, const Eigen::Isometry3d& p_matTransformationLEFTtoWORLD, const Eigen::Isometry3d& p_matTransformationWORLDtoLEFT, const MatrixProjection& p_matProjectionLEFT, const MatrixProjection& p_matProjectionRIGHT, const MatrixProjection& p_matProjectionWORLDtoLEFT, const MatrixProjection& p_matProjectionWORLDtoRIGHT, const UIDFrame& p_uFrame ): uID( p_uID ), matDescriptorReferenceLEFT( p_matDescriptorLEFT ), matDescriptorReferenceRIGHT( p_matDescriptorRIGHT ), dKeyPointSize( p_dKeyPointSize ), vecPointXYZInitial( p_matTransformationLEFTtoWORLD*p_vecPointXYZLEFT ), vecPointXYZOptimized( vecPointXYZInitial ), vecUVReferenceLEFT( p_ptUVLEFT.x, p_ptUVLEFT.y, 1.0 ), vecPointXYZMean( vecPointXYZInitial ), m_matProjectionLEFT( p_matProjectionLEFT ), m_matProjectionRIGHT( p_matProjectionRIGHT ) { vecDescriptorsLEFT.clear( ); vecDescriptorsRIGHT.clear( ); m_vecMeasurements.clear( ); //ds construct filestring and open dump file //char chBuffer[256]; //std::snprintf( chBuffer, 256, "/home/dominik/workspace_catkin/src/vi_mapper/logs/landmarks/landmark%06lu.txt", uID ); //m_pFilePositionOptimization = std::fopen( chBuffer, "w" ); //assert( 0 != m_pFilePositionOptimization ); //ds dump file format //std::fprintf( m_pFilePositionOptimization, "ID_FRAME | ID_LANDMARK | ITERATION MEASUREMENTS INLIERS | ERROR_ARSS | DELTA_XYZ | X Y Z\n" ); //ds add this position addMeasurement( p_uFrame, p_ptUVLEFT, p_ptUVRIGHT, p_matDescriptorLEFT, p_matDescriptorRIGHT, p_vecPointXYZLEFT, p_matTransformationLEFTtoWORLD, p_matTransformationWORLDtoLEFT, p_matProjectionWORLDtoLEFT, p_matProjectionWORLDtoRIGHT ); }
//------------------------------------------- // Entry point for the application //------------------------------------------- void appMain(void) { PRINTF("Starting...\n"); initFullBN(); accelOn(); mdelay(START_DELAY); redLedOn(); for (;;) { uint16_t now = ALARM_TIMER_VALUE(); uint16_t endTime = now + MAIN_LOOP_LENGTH; Packet_t packet; packet.accX = accelReadX(); packet.accY = accelReadY(); packet.accZ = accelReadZ(); // PRINTF("read %d %d %d\n", packet.accX, packet.accY, packet.accZ); uint16_t start = ALARM_TIMER_VALUE(); MEMORY_BARRIER(); addMeasurement(&packet); MEMORY_BARRIER(); uint16_t end = ALARM_TIMER_VALUE(); if (vectorPos == 0) { PRINTF("time to calc = %u ticks\n", end - start); } while (timeAfter16(endTime, ALARM_TIMER_VALUE())); } }
void AnalogSampler::measure() { double v = analogRead(_pin) / 1024.0; addMeasurement(v); }
bool VideoBasedTracker::processImage(cv::Mat frame, cv::Mat grayImage, PoseHandler handler) { m_assertInvariants(); bool done = false; m_frame = frame; m_imageGray = grayImage; //================================================================ // Tracking the points // Threshold the image based on the brightness value that is between // the darkest and brightest pixel in the image. // @todo Make this a parameter. double minVal, maxVal; cv::minMaxLoc(m_imageGray, &minVal, &maxVal); double thresholdValue = 220; cv::threshold(m_imageGray, m_thresholdImage, thresholdValue, 255, CV_THRESH_BINARY); // @todo are any of the above steps already being performed in the blob // detector (if configured in a given way?) // http://docs.opencv.org/master/d0/d7a/classcv_1_1SimpleBlobDetector.html#details // Construct a blob detector and find the blobs in the image. // This set of parameters is optimized for the OSVR HDK prototype // that has exposed LEDs. /// @todo: Make a different set of parameters optimized for the /// Oculus Dk2. /// @todo: Determine the maximum size of a trackable blob by seeing /// when we're so close that we can't view at least four in the /// camera. cv::SimpleBlobDetector::Params params; params.minThreshold = static_cast<float>(thresholdValue); params.maxThreshold = static_cast<float>( thresholdValue + (maxVal - thresholdValue) * 0.3); params.thresholdStep = (params.maxThreshold - params.minThreshold) / 10; params.blobColor = static_cast<uchar>(255); params.filterByColor = false; // Look for bright blobs: there is a bug in this code params.minInertiaRatio = 0.5; params.maxInertiaRatio = 1.0; params.filterByInertia = false; // Do we test for non-elongated blobs? params.minArea = 1; // How small can the blobs be? params.filterByConvexity = false; // Test for convexity? params.filterByCircularity = false; // Test for circularity? params.minDistBetweenBlobs = 3; #if CV_MAJOR_VERSION == 2 cv::Ptr<cv::SimpleBlobDetector> detector = new cv::SimpleBlobDetector(params); #elif CV_MAJOR_VERSION == 3 auto detector = cv::SimpleBlobDetector::create(params); #else #error "Unrecognized OpenCV version!" #endif /// @todo this variable is a candidate for hoisting to member std::vector<cv::KeyPoint> foundKeyPoints; detector->detect(m_imageGray, foundKeyPoints); // @todo: Consider computing the center of mass of a dilated bounding // rectangle around each keypoint to produce a more precise subpixel // localization of each LED. The moments() function may be helpful // with this. // @todo: Estimate the summed brightness of each blob so that we can // detect when they are getting brighter and dimmer. Pass this as // the brightness parameter to the Led class when adding a new one // or augmenting with a new frame. // We allow multiple sets of LEDs, each corresponding to a different // sensor, to be located in the same image. We construct a new set // of LEDs for each and try to find them. It is assumed that they all // have unique ID patterns across all sensors. for (size_t sensor = 0; sensor < m_identifiers.size(); sensor++) { osvrPose3SetIdentity(&m_pose); std::vector<cv::KeyPoint> keyPoints = foundKeyPoints; // Locate the closest blob from this frame to each LED found // in the previous frame. If it is close enough to the nearest // neighbor from last time, we assume that it is the same LED and // update it. If not, we delete the LED from the list. Once we // have matched a blob to an LED, we remove it from the list. If // there are any blobs leftover, we create new LEDs from them. // @todo: Include motion estimate based on Kalman filter along with // model of the projection once we have one built. Note that this // will require handling the lens distortion appropriately. auto led = begin(m_led_groups[sensor]); auto e = end(m_led_groups[sensor]); while (led != e) { double TODO_BLOB_MOVE_THRESHOLD = 10; auto nearest = led->nearest(keyPoints, TODO_BLOB_MOVE_THRESHOLD); if (nearest == keyPoints.end()) { // We have no blob corresponding to this LED, so we need // to delete this LED. led = m_led_groups[sensor].erase(led); } else { // Update the values in this LED and then go on to the // next one. Remove this blob from the list of potential // matches. led->addMeasurement(nearest->pt, nearest->size); keyPoints.erase(nearest); ++led; } } // If we have any blobs that have not been associated with an // LED, then we add a new LED for each of them. // std::cout << "Had " << Leds.size() << " LEDs, " << // keyPoints.size() << " new ones available" << std::endl; for (auto &keypoint : keyPoints) { m_led_groups[sensor].emplace_back(m_identifiers[sensor].get(), keypoint.pt, keypoint.size); } //================================================================== // Compute the pose of the HMD w.r.t. the camera frame of reference. bool gotPose = false; if (m_estimators[sensor]) { // Get an estimated pose, if we have enough data. OSVR_PoseState pose; if (m_estimators[sensor]->EstimatePoseFromLeds( m_led_groups[sensor], pose)) { // Project the expected locations of the beacons // into the image and then compute the error between the // expected locations and the visible locations for all of // the visible beacons. If they are too far off, cancel the // pose. std::vector<cv::Point2f> imagePoints; m_estimators[sensor]->ProjectBeaconsToImage(imagePoints); for (auto &led : m_led_groups[sensor]) { auto label = std::to_string(led.getOneBasedID()); cv::Point where = led.getLocation(); } // XXX m_pose = pose; handler(static_cast<unsigned>(sensor), pose); gotPose = true; } } #ifdef VBHMD_DEBUG // Don't display the debugging info every frame, or we can't go fast // enough. static int count = 0; if (++count == 11) { // Draw detected blobs as red circles. cv::drawKeypoints(m_frame, keyPoints, m_imageWithBlobs, cv::Scalar(0, 0, 255), cv::DrawMatchesFlags::DRAW_RICH_KEYPOINTS); // Label the keypoints with their IDs. for (auto &led : m_led_groups[sensor]) { // Print 1-based LED ID for actual LEDs auto label = std::to_string(led.getOneBasedID()); cv::Point where = led.getLocation(); where.x += 1; where.y += 1; cv::putText(m_imageWithBlobs, label, where, cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 0, 255)); } // If we have a transform, reproject all of the points from the // model space (the LED locations) back into the image and // display them on the blob image in green. if (gotPose) { std::vector<cv::Point2f> imagePoints; m_estimators[sensor]->ProjectBeaconsToImage(imagePoints); size_t n = imagePoints.size(); for (size_t i = 0; i < n; ++i) { // Print 1-based LED IDs auto label = std::to_string(i + 1); auto where = imagePoints[i]; where.x += 1; where.y += 1; cv::putText(m_imageWithBlobs, label, where, cv::FONT_HERSHEY_SIMPLEX, 0.5, cv::Scalar(0, 255, 0)); } } // Pick which image to show and show it. if (m_frame.data) { std::ostringstream windowName; windowName << "Sensor" << sensor; cv::imshow(windowName.str().c_str(), *m_shownImage); int key = cv::waitKey(1); switch (key) { case 'i': // Show the input image. m_shownImage = &m_frame; break; case 't': // Show the thresholded image. m_shownImage = &m_thresholdImage; break; case 'b': // Show the blob image. m_shownImage = &m_imageWithBlobs; break; case 'q': // Indicate we want to quit. done = true; break; } } // Report the pose, if we got one if (gotPose) { std::cout << "Pos (sensor " << sensor << "): " << m_pose.translation.data[0] << ", " << m_pose.translation.data[1] << ", " << m_pose.translation.data[2] << std::endl; } count = 0; } #endif } m_assertInvariants(); return done; }