Esempio n. 1
0
void _ROITracker::track(void)
{
	Frame* pFrame;
	Mat* pMat;

	NULL_(m_pStream);
	CHECK_(!m_bTracking);
	CHECK_(m_pTracker.empty());

	pFrame = m_pStream->bgr();
	NULL_(pFrame);
	CHECK_(!pFrame->isNewerThan(m_pFrame));
	m_pFrame->update(pFrame);

	pMat = m_pFrame->getCMat();
	if (pMat->empty())
		return;

	if (m_newROI.width > 0)
	{
		m_pTracker.release();
		m_pTracker = Tracker::create("KCF");

		m_ROI = m_newROI;
		m_pTracker->init(*pMat, m_ROI);
		m_newROI.width = 0;
	}

	// update the tracking result
	m_pTracker->update(*pMat, m_ROI);

}
Esempio n. 2
0
void RC_visualFollow::onMouse(MOUSE* pMouse)
{
	NULL_(pMouse);

	//update UI
	if (m_ROImode == MODE_ASSIST)
	{
		NULL_(m_pUIassist);
		onMouseAssist(pMouse, m_pUIassist->onMouse(pMouse));
	}
	else if (m_ROImode == MODE_DRAWRECT)
	{
		NULL_(m_pUIdrawRect);
		onMouseDrawRect(pMouse, m_pUIdrawRect->onMouse(pMouse));
	}
}
Esempio n. 3
0
void _SSD::detectFrame(void)
{
    OBJECT obj;
    Frame* pFrame;
    unsigned int iClass;
    unsigned int i;

    NULL_(m_pStream);
    NULL_(m_pUniverse);

    pFrame = m_pStream->bgr();
    NULL_(pFrame);
    CHECK_(pFrame->empty());
    if (!pFrame->isNewerThan(m_pFrame))return;
    m_pFrame->update(pFrame);

    cv::cuda::GpuMat* pImg = m_pFrame->getGMat();
    std::vector<vector<float> > detections = detect(m_pFrame);

    /* Print the detection results. */
    for (i = 0; i < detections.size(); ++i)
    {
        const vector<float>& d = detections[i];
        // Detection format: [image_id, label, score, xmin, ymin, xmax, ymax].
//		CHECK_EQ(d.size(), 7);
//		float size = d.size();
        const float score = d[2];

        if (score < m_confidence_threshold)continue;

        iClass = static_cast<int>(d[1])-1;
        if(iClass >= labels_.size())continue;

        obj.m_iClass = iClass;
        obj.m_bbox.m_x = d[3] * pImg->cols;
        obj.m_bbox.m_y = d[4] * pImg->rows;
        obj.m_bbox.m_z = d[5] * pImg->cols;
        obj.m_bbox.m_w = d[6] * pImg->rows;
        obj.m_camSize.m_x = pImg->cols;
        obj.m_camSize.m_y = pImg->rows;
        obj.m_dist = 0.0;
        obj.m_prob = 0.0;

        m_pUniverse->addObject(&obj);
    }

}
Esempio n. 4
0
void _Bullseye::detectCircleHough(void)
{
	if(!m_pStream)return;

	Frame* pFrame = m_pStream->hsv();
	NULL_(pFrame);
	CHECK_(pFrame->empty());
	if(!pFrame->isNewerThan(m_pFrame))return;
	m_pFrame->update(pFrame);

//	cv::Mat bgr_image = *m_pFrame->getCMat();
//	cv::medianBlur(bgr_image, bgr_image, m_kSize);

	// Convert input image to HSV
//	cv::Mat hsv_image;
//	cv::cvtColor(bgr_image, hsv_image, cv::COLOR_BGR2HSV);

	// Convert input image to HSV
	cv::Mat hsv_image = *m_pFrame->getCMat();
//	cv::medianBlur(hsv_image, hsv_image, 3);

	// Threshold the HSV image, keep only the red pixels
	cv::Mat lower_red_hue_range;
	cv::Mat upper_red_hue_range;
	cv::inRange(hsv_image, cv::Scalar(0, 100, 100), cv::Scalar(10, 255, 255), lower_red_hue_range);
	cv::inRange(hsv_image, cv::Scalar(160, 100, 100), cv::Scalar(179, 255, 255), upper_red_hue_range);

	// Combine the above two images
	cv::Mat red_hue_image;
	cv::addWeighted(lower_red_hue_range, 1.0, upper_red_hue_range, 1.0, 0.0, red_hue_image);
	cv::GaussianBlur(red_hue_image, red_hue_image, cv::Size(9, 9), 2, 2);

	// Use the Hough transform to detect circles in the combined threshold image
	std::vector<cv::Vec3f> circles;
	cv::HoughCircles(red_hue_image, circles, CV_HOUGH_GRADIENT, 1, m_houghMinDist, m_houghParam1, m_houghParam2, m_houghMinR, m_houghMaxR);


	m_numCircle = 0;
	if(circles.size() == 0)return;

	for(size_t current_circle = 0; current_circle < circles.size(); ++current_circle)
	{
		m_pCircle[m_numCircle].m_x = circles[current_circle][0];
		m_pCircle[m_numCircle].m_y = circles[current_circle][1];
		m_pCircle[m_numCircle].m_z = circles[current_circle][2];
		m_numCircle++;

		if (m_numCircle == NUM_MARKER)
		{
			break;
		}

	}
}
void _Lightware_SF40_sender::MBS(uint8_t MBS)
{
	NULL_(m_pSerialPort);
	CHECK_(!m_pSerialPort->isOpen());

	if(MBS>3)MBS = 3;

	char str[32];
	sprintf(str, "#MBS,%d\x0d\x0a", MBS);
	m_pSerialPort->write((uint8_t*)str, strlen(str));
}
Esempio n. 6
0
void _AutoPilot::onMouse(MOUSE* pMouse)
{
    NULL_(pMouse);

    for(int i=0; i<m_nAction; i++)
    {
        if(*m_pAction[i]->getClass()=="RC_visualFollow")
        {
            ((RC_visualFollow*)m_pAction[i])->onMouse(pMouse);
        }
    }
}
Esempio n. 7
0
void APMrover_base::sendHeartbeat(void)
{
	NULL_(m_pMavlink);

	//Sending Heartbeat at 1Hz
	uint64_t timeNow = get_time_usec();
	CHECK_(timeNow - m_lastHeartbeat < USEC_1SEC);

	m_pMavlink->sendHeartbeat();
	m_lastHeartbeat = timeNow;

	LOG(INFO)<<"APMrover HEARTBEAT:"<<++m_iHeartbeat;
}
Esempio n. 8
0
void HM_follow::update(void)
{
	this->ActionBase::update();

	NULL_(m_pHM);
	NULL_(m_pUniv);
	CHECK_(m_pAM->getCurrentStateIdx() != m_iActiveState);

	//get visual target and decide motion
	m_pTarget = m_pUniv->getObjectByClass(m_targetClass);

	if (m_pTarget == NULL)
	{
		//no target found, stop and standby TODO: go back to work
		m_pHM->m_motorPwmL = 0;
		m_pHM->m_motorPwmR = 0;
		m_pHM->m_bSpeaker = false;
	}
	else
	{
		m_pTargetX->input(m_pTarget->m_bbox.midX());
		m_pTargetY->input(m_pTarget->m_bbox.midY());
		m_pTargetArea->input(m_pTarget->m_bbox.area());

		//forward or backward
		int rpmSpeed = (m_destArea*m_pTarget->m_camSize.area() - m_pTargetArea->v()) * m_speedP;

		//steering
		int rpmSteer = (m_destX*m_pTarget->m_camSize.m_x - m_pTargetX->v()) * m_steerP;

		m_pHM->m_motorPwmL = rpmSpeed - rpmSteer;
		m_pHM->m_motorPwmR = rpmSpeed + rpmSteer;
		m_pHM->m_bSpeaker = true;
	}

	m_pHM->updateCAN();
}
Esempio n. 9
0
void APMrover_base::sendHeartbeat(void)
{
	NULL_(m_pMavlink);

	//Sending Heartbeat at 1Hz
	uint64_t timeNow = get_time_usec();
	CHECK_(timeNow - m_lastHeartbeat < USEC_1SEC);

	m_pMavlink->sendHeartbeat();
	m_lastHeartbeat = timeNow;

#ifdef MAVLINK_DEBUG
	printf("<- OpenKAI HEARTBEAT:%d\n", (++m_iHeartbeat));
#endif
}
Esempio n. 10
0
void _Lightware_SF40_sender::LD(void)
{
	NULL_(m_pSerialPort);
	CHECK_(!m_pSerialPort->isOpen());

	//?LD,aaa.a<CR><LF> <space>dd.dd<CR><LF>

	char str[128];

	for(double angle=0; angle<DEG_AROUND; angle+=m_dAngle)
	{
		sprintf(str, "?LD,%.1f\x0d\x0a", angle);
		m_pSerialPort->write((uint8_t*)str, strlen(str));
	}

}
Esempio n. 11
0
void APMrover_base::sendSteerThrust(void)
{
	NULL_(m_pMavlink);
	m_pMavlink->command_long_doSetPositionYawThrust(m_steer, m_thrust);

}
Esempio n. 12
0
void _BgFg::detect(void)
{
	int i;
	Point2f center;
	float radius;
	vector< vector< Point > > contours;
	vector<Vec3f> circles;
	UMat matThresh;
	Frame* pRGB;

	if(!m_pCamStream)return;
	pRGB = m_pCamStream->bgr();//m_pCamStream->m_pFrameL;
	NULL_(pRGB);
	CHECK_(pRGB->empty());

	m_pBgSubtractor->apply(*pRGB->getGMat(), m_gFg);
//	m_pBgSubtractor->getBackgroundImage(m_gBg);

	pRGB->getGMat()->download(m_Mat);
	m_gFg.download(matThresh);

	//Find the contours
	findContours(matThresh, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_NONE);
//	drawContours(m_Mat, contours, -1, Scalar(0, 255, 0), 2);

	//Find marker
	m_numAllMarker = 0;
	for (i=0; i<contours.size(); i++)
	{
		minEnclosingCircle(contours[i], center, radius);
//		circle(m_Mat, center, radius, Scalar(0, 255, 0), 1);

		//New marker found
		if (contourArea(contours[i]) < 10)continue;
		if (radius < m_minSize)continue;

		circle(m_Mat, center, radius, Scalar(0, 255, 0), 2);

		m_pAllMarker[m_numAllMarker].m_x = center.x;
		m_pAllMarker[m_numAllMarker].m_y = center.y;
		m_pAllMarker[m_numAllMarker].m_z = radius;
		m_pAllMarker[m_numAllMarker].m_w = abs(center.x - m_objROIPos.m_x) + abs(center.y - m_objROIPos.m_y);
		m_numAllMarker++;

		if (m_numAllMarker == NUM_MARKER)
		{
			break;
		}
	}

	//Failed to detect any marker
	if (m_numAllMarker <= 0)
	{
		m_objLockLevel = LOCK_LEVEL_NONE;
		return;
	}

	//TODO: use the center point of all points as the vehicle position

	//Sorting the markers from near to far
//	for (i=1; i<m_numAllMarker; i++)
//	{
//		for (j=i-1; j>=0; j--)
//		{
//			if (m_pAllMarker[j+1].m_w > m_pAllMarker[j].m_w)
//			{
//				break;
//			}
//
//			v4tmp = m_pAllMarker[j];
//			m_pAllMarker[j] = m_pAllMarker[j+1];
//			m_pAllMarker[j+1] = v4tmp;
//		}
//	}


		m_objPos.m_x = m_pAllMarker[0].m_x;
		m_objPos.m_y = m_pAllMarker[0].m_y;
		m_objPos.m_z = m_pAllMarker[0].m_z;
		m_objROIPos = m_objPos;

		//only position is locked
		m_objLockLevel = LOCK_LEVEL_POS;

	//Determine the center position and size of object
/*	m_objPos.m_x = (m_pAllMarker[1].m_x + m_pAllMarker[0].m_x)*0.5;
	m_objPos.m_y = (m_pAllMarker[1].m_y + m_pAllMarker[0].m_y)*0.5;
	m_objPos.m_z = abs(m_pAllMarker[1].m_x - m_pAllMarker[0].m_x);
	m_objROIPos = m_objPos;
	m_objLockLevel = LOCK_LEVEL_SIZE;

	m_objLockLevel = LOCK_LEVEL_ATT;
*/
}
Esempio n. 13
0
void RC_visualFollow::update(void)
{
	this->ActionBase::update();

	NULL_(m_pRC);
	NULL_(m_pROITracker);
	CHECK_(m_pAM->getCurrentStateIdx() != m_iActiveState);

	if (m_pROITracker->m_bTracking == false)
	{
		m_pRCconfig->m_rcRoll.neutral();
		m_pRCconfig->m_rcPitch.neutral();
		m_roll.resetErr();
		m_pitch.resetErr();
//		m_pCtrl->m_pRC->rc_overide(m_pCtrl->m_nRC, m_pCtrl->m_pRC);
		return;
	}

	double posRoll;
	double posPitch;
	double ovDTime;

	RC_PID* pidRoll = &m_pRCconfig->m_pidRoll;
	RC_PID* pidPitch = &m_pRCconfig->m_pidPitch;
	RC_PID* pidAlt = &m_pRCconfig->m_pidAlt;
	RC_PID* pidYaw = &m_pRCconfig->m_pidYaw;

	RC_CHANNEL* pRCroll = &m_pRCconfig->m_rcRoll;
	RC_CHANNEL* pRCpitch = &m_pRCconfig->m_rcPitch;
	RC_CHANNEL* pRCalt = &m_pRCconfig->m_rcAlt;
	RC_CHANNEL* pRCyaw = &m_pRCconfig->m_rcYaw;

	ovDTime = (1.0 / m_pROITracker->m_dTime) * 1000; //ms
	posRoll = m_roll.m_pos;
	posPitch = m_pitch.m_pos;

	//Update pos from ROI tracker
	m_roll.m_pos = m_pROITracker->m_ROI.x + m_pROITracker->m_ROI.width * 0.5;
	m_pitch.m_pos = m_pROITracker->m_ROI.y + m_pROITracker->m_ROI.height * 0.5;

	//Update current position with trajectory estimation
	posRoll = m_roll.m_pos + (m_roll.m_pos - posRoll) * pidRoll->m_dT * ovDTime;
	posPitch = m_pitch.m_pos
			+ (m_pitch.m_pos - posPitch) * pidPitch->m_dT * ovDTime;

	//Roll
	m_roll.m_errOld = m_roll.m_err;
	m_roll.m_err = m_roll.m_targetPos - posRoll;
	m_roll.m_errInteg += m_roll.m_err;
	pRCroll->m_pwm = pRCroll->m_pwmN + pidRoll->m_P * m_roll.m_err
			+ pidRoll->m_D * (m_roll.m_err - m_roll.m_errOld) * ovDTime
			+ constrain(pidRoll->m_I * m_roll.m_errInteg, pidRoll->m_Imax,
					-pidRoll->m_Imax);
	pRCroll->m_pwm = constrain(pRCroll->m_pwm, pRCroll->m_pwmL,
			pRCroll->m_pwmH);

	//Pitch
	m_pitch.m_errOld = m_pitch.m_err;
	m_pitch.m_err = m_pitch.m_targetPos - posPitch;
	m_pitch.m_errInteg += m_pitch.m_err;
	pRCpitch->m_pwm = pRCpitch->m_pwmN + pidPitch->m_P * m_pitch.m_err
			+ pidPitch->m_D * (m_pitch.m_err - m_pitch.m_errOld) * ovDTime
			+ constrain(pidPitch->m_I * m_pitch.m_errInteg, pidPitch->m_Imax,
					-pidPitch->m_Imax);
	pRCpitch->m_pwm = constrain(pRCpitch->m_pwm, pRCpitch->m_pwmL,
			pRCpitch->m_pwmH);

	//RC output
//	m_pCtrl->m_pRC->rc_overide(m_pCtrl->m_nRC, m_pCtrl->m_pRC);
	return;

}
Esempio n. 14
0
void RC_visualFollow::onMouseDrawRect(MOUSE* pMouse, BUTTON* pBtn)
{
	NULL_(pMouse);

	Rect2d roi;
	int ROIhalf;

	switch (pMouse->m_event)
	{
	case EVENT_LBUTTONDOWN:
		if (!pBtn)
		{
			m_pROITracker->tracking(false);
			m_ROI.m_x = pMouse->m_x;
			m_ROI.m_y = pMouse->m_y;
			m_ROI.m_z = pMouse->m_x;
			m_ROI.m_w = pMouse->m_y;
			m_bSelect = true;
			return;
		}

		if (pBtn->m_name == "MODE")
		{
			m_ROI.m_x = 0;
			m_ROI.m_y = 0;
			m_ROI.m_z = 0;
			m_ROI.m_w = 0;
			m_pROITracker->tracking(false);
			m_bSelect = false;
			m_ROImode = MODE_ASSIST;
			return;
		}

		break;
	case EVENT_MOUSEMOVE:
		CHECK_(!m_bSelect);

		m_ROI.m_z = pMouse->m_x;
		m_ROI.m_w = pMouse->m_y;
		break;
	case EVENT_LBUTTONUP:
		roi = getMouseROI(m_ROI);
		if (roi.width < m_ROIsizeFrom || roi.height < m_ROIsizeFrom)
		{
			m_ROI.m_x = 0;
			m_ROI.m_y = 0;
			m_ROI.m_z = 0;
			m_ROI.m_w = 0;
		}
		else
		{
			m_pROITracker->setROI(roi);
			m_pROITracker->tracking(true);
		}
		m_bSelect = false;
		break;
	case EVENT_RBUTTONDOWN:
		break;
	default:
		break;
	}

}
Esempio n. 15
0
void RC_visualFollow::onMouseAssist(MOUSE* pMouse, BUTTON* pBtn)
{
	NULL_(pMouse);

	Rect2d roi;
	int ROIhalf;

	switch (pMouse->m_event)
	{
	case EVENT_LBUTTONDOWN:
		if (!pBtn)
		{
			ROIhalf = m_ROIsize / 2;
			m_ROI.m_x = pMouse->m_x - ROIhalf;
			m_ROI.m_y = pMouse->m_y - ROIhalf;
			m_ROI.m_z = pMouse->m_x + ROIhalf;
			m_ROI.m_w = pMouse->m_y + ROIhalf;
			roi = getMouseROI(m_ROI);
			m_pROITracker->setROI(roi);
			m_pROITracker->tracking(true);
			m_bSelect = true;
			return;
		}

		if (pBtn->m_name == "CLR")
		{
			//Clear ROI
			m_pROITracker->tracking(false);
			m_bSelect = false;
			return;
		}

		if (pBtn->m_name == "+")
		{
			//Magnify the ROI size
			m_bSelect = false;

			CHECK_(m_ROIsize >= m_ROIsizeTo);
			m_ROIsize += m_ROIsizeStep;
			CHECK_(!m_pROITracker->m_bTracking);

			roi.x = m_pROITracker->m_ROI.x + m_pROITracker->m_ROI.width / 2;
			roi.y = m_pROITracker->m_ROI.y + m_pROITracker->m_ROI.height / 2;
			ROIhalf = m_ROIsize / 2;

			m_ROI.m_x = roi.x - ROIhalf;
			m_ROI.m_y = roi.y - ROIhalf;
			m_ROI.m_z = roi.x + ROIhalf;
			m_ROI.m_w = roi.y + ROIhalf;
			roi = getMouseROI(m_ROI);
			m_pROITracker->setROI(roi);
			return;
		}

		if (pBtn->m_name == "-")
		{
			//Shrink the ROI size
			m_bSelect = false;

			CHECK_(m_ROIsize <= m_ROIsizeFrom);
			m_ROIsize -= m_ROIsizeStep;
			CHECK_(!m_pROITracker->m_bTracking);

			roi.x = m_pROITracker->m_ROI.x + m_pROITracker->m_ROI.width / 2;
			roi.y = m_pROITracker->m_ROI.y + m_pROITracker->m_ROI.height / 2;
			ROIhalf = m_ROIsize / 2;

			m_ROI.m_x = roi.x - ROIhalf;
			m_ROI.m_y = roi.y - ROIhalf;
			m_ROI.m_z = roi.x + ROIhalf;
			m_ROI.m_w = roi.y + ROIhalf;
			roi = getMouseROI(m_ROI);
			m_pROITracker->setROI(roi);
			return;
		}

		if (pBtn->m_name == "MODE")
		{
			m_ROI.m_x = 0;
			m_ROI.m_y = 0;
			m_ROI.m_z = 0;
			m_ROI.m_w = 0;
			m_pROITracker->tracking(false);
			m_bSelect = false;
			m_ROImode = MODE_DRAWRECT;
			return;
		}

		break;
	case EVENT_MOUSEMOVE:
		CHECK_(!m_bSelect);

		ROIhalf = m_ROIsize / 2;
		m_ROI.m_x = pMouse->m_x - ROIhalf;
		m_ROI.m_y = pMouse->m_y - ROIhalf;
		m_ROI.m_z = pMouse->m_x + ROIhalf;
		m_ROI.m_w = pMouse->m_y + ROIhalf;
		roi = getMouseROI(m_ROI);
		m_pROITracker->setROI(roi);
		m_pROITracker->tracking(true);
		break;
	case EVENT_LBUTTONUP:
		m_bSelect = false;
		break;
	case EVENT_RBUTTONDOWN:
		break;
	default:
		break;
	}

}