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);
}
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));
}
}
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);
}
}
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));
}
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);
}
}
}
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;
}
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();
}
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
}
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));
}
}
void APMrover_base::sendSteerThrust(void)
{
NULL_(m_pMavlink);
m_pMavlink->command_long_doSetPositionYawThrust(m_steer, m_thrust);
}
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;
*/
}
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;
}
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;
}
}
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;
}
}
