void MoreAccurateMotionWobbleSuppressorGpu::suppress(int idx, const gpu::GpuMat &frame, gpu::GpuMat &result)
{
    CV_Assert(motions_ && stabilizationMotions_);

    if (idx % period_ == 0)
    {
        result = frame;
        return;
    }

    int k1 = idx / period_ * period_;
    int k2 = std::min(k1 + period_, frameCount_ - 1);

    Mat S1 = (*stabilizationMotions_)[idx];

    Mat ML = S1 * getMotion(k1, idx, *motions2_) * getMotion(k1, idx, *motions_).inv() * S1.inv();
    Mat MR = S1 * getMotion(idx, k2, *motions2_).inv() * getMotion(idx, k2, *motions_) * S1.inv();

    gpu::calcWobbleSuppressionMaps(k1, idx, k2, frame.size(), ML, MR, mapx_, mapy_);

    if (result.data == frame.data)
        result = gpu::GpuMat(frame.size(), frame.type());

    gpu::remap(frame, result, mapx_, mapy_, INTER_LINEAR, BORDER_REPLICATE);
}
Пример #2
0
static void saveMotions(
        int frameCount, const std::vector<Mat> &motions, const std::vector<Mat> &stabilizationMotions)
{
    std::ofstream fm("log_motions.csv");
    for (int i = 0; i < frameCount - 1; ++i)
    {
        Mat_<float> M = at(i, motions);
        fm << M(0,0) << " " << M(0,1) << " " << M(0,2) << " "
           << M(1,0) << " " << M(1,1) << " " << M(1,2) << " "
           << M(2,0) << " " << M(2,1) << " " << M(2,2) << std::endl;
    }
    std::ofstream fo("log_orig.csv");
    for (int i = 0; i < frameCount; ++i)
    {
        Mat_<float> M = getMotion(0, i, motions);
        fo << M(0,0) << " " << M(0,1) << " " << M(0,2) << " "
           << M(1,0) << " " << M(1,1) << " " << M(1,2) << " "
           << M(2,0) << " " << M(2,1) << " " << M(2,2) << std::endl;
    }
    std::ofstream fs("log_stab.csv");
    for (int i = 0; i < frameCount; ++i)
    {
        Mat_<float> M = stabilizationMotions[i] * getMotion(0, i, motions);
        fs << M(0,0) << " " << M(0,1) << " " << M(0,2) << " "
           << M(1,0) << " " << M(1,1) << " " << M(1,2) << " "
           << M(2,0) << " " << M(2,1) << " " << M(2,2) << std::endl;
    }
}
void MoreAccurateMotionWobbleSuppressor::suppress(int idx, const Mat &frame, Mat &result)
{
    CV_Assert(motions_ && stabilizationMotions_);

    if (idx % period_ == 0)
    {
        result = frame;
        return;
    }

    int k1 = idx / period_ * period_;
    int k2 = std::min(k1 + period_, frameCount_ - 1);

    Mat S1 = (*stabilizationMotions_)[idx];

    Mat_<float> ML = S1 * getMotion(k1, idx, *motions2_) * getMotion(k1, idx, *motions_).inv() * S1.inv();
    Mat_<float> MR = S1 * getMotion(idx, k2, *motions2_).inv() * getMotion(idx, k2, *motions_) * S1.inv();

    mapx_.create(frame.size());
    mapy_.create(frame.size());

    float xl, yl, zl, wl;
    float xr, yr, zr, wr;

    for (int y = 0; y < frame.rows; ++y)
    {
        for (int x = 0; x < frame.cols; ++x)
        {
            xl = ML(0,0)*x + ML(0,1)*y + ML(0,2);
            yl = ML(1,0)*x + ML(1,1)*y + ML(1,2);
            zl = ML(2,0)*x + ML(2,1)*y + ML(2,2);
            xl /= zl; yl /= zl;
            wl = float(idx - k1);

            xr = MR(0,0)*x + MR(0,1)*y + MR(0,2);
            yr = MR(1,0)*x + MR(1,1)*y + MR(1,2);
            zr = MR(2,0)*x + MR(2,1)*y + MR(2,2);
            xr /= zr; yr /= zr;
            wr = float(k2 - idx);

            mapx_(y,x) = (wr * xl + wl * xr) / (wl + wr);
            mapy_(y,x) = (wr * yl + wl * yr) / (wl + wr);
        }
    }

    if (result.data == frame.data)
        result = Mat(frame.size(), frame.type());

    remap(frame, result, mapx_, mapy_, INTER_LINEAR, BORDER_REPLICATE);
}
Пример #4
0
int main(void)
{
	
	ConfigPins();
	initUART();
	initSysTick();	
	RValue = initAccel();	
	printInteger(RValue);
	if (RValue == 0) {
		printString("Boldly going :)\r\n");
	} else {
		printString("Inertia sensors offline :(\r\n");
	}
	while (1) { 		
		Temperature = getTemperature();		
		printShort(Temperature);		
		printString(" ");					
		getMotion(&m);
		printShort(m.x_a);		
		printString(" ");
		printShort(m.y_a);		
		printString(" ");
		printShort(m.z_a);		
		printString(" ");		
		printString("\r\n");				
		GPIO0DATA ^= BIT2;				
		delay(10000);
	}

	return 0;
}
Пример #5
0
bool QICMotionCapture::OnReadEvent() {

  //INFO("head of on OnreadEvent");
  struct input_event ev[64];
  int rd;
  rd = read(GetDeviceFD(), ev, sizeof(struct input_event) * 64);

  if (rd < (int) sizeof(struct input_event)) {
    printf("expected %d bytes, got %d\n", (int) sizeof(struct input_event), rd);
    perror("\nevtest: error reading");
    return 1;
  }

  rect_t mymotion[3];
  unsigned long ts;
  int numofmotion=getMotion(mymotion, ts);
//  INFO("numofmotion %d",numofmotion);

  uint8_t motionbuffer[4*numofmotion];
  for(int i=0;i<numofmotion;i++){
    motionbuffer[i]=mymotion->x;
    motionbuffer[i+1]=mymotion->y;
    motionbuffer[i+2]=mymotion->width;
    motionbuffer[i+3]=mymotion->height;
  }
  INFO("ts: %lu last ts:%lu", ts, _CtrlTs);
  if(_CtrlTs==0)
    _CtrlTs=ts-(1000/_FPSControl+1);

  if((ts-_CtrlTs)>(1000/_FPSControl)) {
    _CtrlTs=ts;
    SendDataToCarriers(motionbuffer,4*numofmotion, ts, HW_DATA_MOTION);
   }
  return true;
 }
Пример #6
0
void getCommand(){
	
	packageCounter++;
	
	if(packageCounter == 4){
		readType();
	}
	
	if(packageCounter == packageSize){
		
		packageCounter = 0;
		packageSize = 4;
		
		getMotion(package);
	}
}
Пример #7
0
WeightedAnimation::WeightedAnimation(BVH* newBVH,const QString& bvhFile) : Animation(newBVH, bvhFile)
{
  int frames_count = newBVH->lastLoadedNumberOfFrames;
  _tPosed = checkTPosed(frames);

  if(frames_count < 2*MIX_IN_OUT)
    _mixIn = _mixOut = frames_count/2;
  else
    _mixIn = _mixOut = MIX_IN_OUT;

  frameWeights = new int[frames_count];
  for(int i=0; i<frames_count; i++)
    frameWeights[i] = 50;

  linearBones = new QMap<QString, BVHNode*>();
  linearBones->insert(positionNode->name(), positionNode);
  initializeLinearBonesHelper(getMotion());
}
Пример #8
0
	void SonicXNFile::setFileMode(XNFileMode target_file_mode) {
		SonicXNObject  *object=getObject();
		SonicXNTexture *texture=getTexture();
		SonicXNEffect  *effect=getEffect();
		SonicXNBones   *bones=getBones();
		SonicXNMotion  *motion=getMotion();

		file_mode = target_file_mode;

		setHeaders();

		if (info) {
			info->setHeader(header_info);
			info->setFileMode(file_mode);
		}

		if (object)  {
			object->setFileMode(file_mode);
			object->setHeader(header_object);
		}
		if (texture) {
			texture->setFileMode(file_mode);
			texture->setHeader(header_texture);
		}
		if (effect)  {
			effect->setFileMode(file_mode);
			effect->setHeader(header_effect);
		}
		if (bones)   {
			bones->setFileMode(file_mode);
			bones->setHeader(header_bones);
		}
		if (motion)  {
			motion->setFileMode(file_mode);
			motion->setHeader(header_motion);
		}

		if (offset_table) offset_table->setFileMode(file_mode);
		if (footer) footer->setFileMode(file_mode);
		if (end) end->setFileMode(file_mode);
	}
Пример #9
0
bool QICMotionCapture2::OnReadEvent() {

  /* call getMotion and deliver to carrier */
  rect_t mymotion[3];
  unsigned long ts;
  int numofmotion=getMotion(mymotion, ts);
//  INFO("numofmotion %d",numofmotion);

  uint8_t motionbuffer[4*numofmotion];
  for(int i=0;i<numofmotion;i++){
    motionbuffer[i]=mymotion->x;
    motionbuffer[i+1]=mymotion->y;
    motionbuffer[i+2]=mymotion->width;
    motionbuffer[i+3]=mymotion->height;
  }
  /*INFO("ts: %lu last ts:%lu", ts, _CtrlTs);
  if(_CtrlTs==0)
    _CtrlTs=ts-(1000/_FPSControl+1);

  if((ts-_CtrlTs)>(1000/_FPSControl)) {
    _CtrlTs=ts;
    SendDataToCarriers(motionbuffer,4*numofmotion, ts, HW_DATA_MOTION);
   }*/

  _motionBox.tsCache[_motionBox.currentIndex]=ts;
  _motionBox.currentIndex++;
  _motionBox.currentIndex%=_eventBufferSize;
  uint8_t boxCounter;
  uint8_t hitCounter=0;
  for (boxCounter=0;boxCounter<_eventBufferSize;boxCounter++){
    if((ts-_motionBox.tsCache[boxCounter]) < _eventInMiliSec)
      hitCounter++;
  }
  if(hitCounter >= _eventCountThreshold)
    SendDataToCarriers(motionbuffer,4*numofmotion, ts, HW_DATA_MOTION);
  return true;
 }
Пример #10
0
void WeightingDeblurer::deblur(int idx, Mat &frame)
{
    CV_Assert(frame.type() == CV_8UC3);

    bSum_.create(frame.size());
    gSum_.create(frame.size());
    rSum_.create(frame.size());
    wSum_.create(frame.size());

    for (int y = 0; y < frame.rows; ++y)
    {
        for (int x = 0; x < frame.cols; ++x)
        {
            Point3_<uchar> p = frame.at<Point3_<uchar> >(y,x);
            bSum_(y,x) = p.x;
            gSum_(y,x) = p.y;
            rSum_(y,x) = p.z;
            wSum_(y,x) = 1.f;
        }
    }

    for (int k = idx - radius_; k <= idx + radius_; ++k)
    {
        const Mat &neighbor = at(k, *frames_);
        float bRatio = at(idx, *blurrinessRates_) / at(k, *blurrinessRates_);
        Mat_<float> M = getMotion(idx, k, *motions_);

        if (bRatio > 1.f)
        {
            for (int y = 0; y < frame.rows; ++y)
            {
                for (int x = 0; x < frame.cols; ++x)
                {
                    int x1 = cvRound(M(0,0)*x + M(0,1)*y + M(0,2));
                    int y1 = cvRound(M(1,0)*x + M(1,1)*y + M(1,2));

                    if (x1 >= 0 && x1 < neighbor.cols && y1 >= 0 && y1 < neighbor.rows)
                    {
                        const Point3_<uchar> &p = frame.at<Point3_<uchar> >(y,x);
                        const Point3_<uchar> &p1 = neighbor.at<Point3_<uchar> >(y1,x1);
                        float w = bRatio * sensitivity_ /
                                (sensitivity_ + std::abs(intensity(p1) - intensity(p)));
                        bSum_(y,x) += w * p1.x;
                        gSum_(y,x) += w * p1.y;
                        rSum_(y,x) += w * p1.z;
                        wSum_(y,x) += w;
                    }
                }
            }
        }
    }

    for (int y = 0; y < frame.rows; ++y)
    {
        for (int x = 0; x < frame.cols; ++x)
        {
            float wSumInv = 1.f / wSum_(y,x);
            frame.at<Point3_<uchar> >(y,x) = Point3_<uchar>(
                    static_cast<uchar>(bSum_(y,x)*wSumInv),
                    static_cast<uchar>(gSum_(y,x)*wSumInv),
                    static_cast<uchar>(rSum_(y,x)*wSumInv));
        }
    }
}
Пример #11
0
void ompl::geometric::LBTRRT::considerEdge(Motion *parent, Motion *child, double c)
{
    // optimization - check if the bounded approximation invariant
    // will be violated after the edge insertion (at least for the child node)
    // if this is the case - perform the local planning
    // this prevents the update of the graph due to the edge insertion and then the re-update as it is removed
    double potential_cost = parent->costLb_ + c;
    if (child->costApx_ > (1 + epsilon_) * potential_cost)
        if (!checkMotion(parent, child))
            return;

    // update lowerBoundGraph_
    std::list<std::size_t> affected;

    lowerBoundGraph_.addEdge(parent->id_, child->id_, c, true, affected);

    // now, check if the bounded apprimation invariant has been violated for each affected vertex
    // insert them into a priority queue ordered according to the lb cost
    std::list<std::size_t>::iterator    iter;
    IsLessThanLB    isLessThanLB(this);
    Lb_queue        queue(isLessThanLB);

    for (iter = affected.begin(); iter != affected.end(); ++iter)
    {
        Motion *m = getMotion(*iter);
        m->costLb_ = lowerBoundGraph_.getShortestPathCost(*iter);
        if (m->costApx_ > (1 + epsilon_) * m->costLb_)
            queue.insert(m);
    }

    while (queue.empty() == false)
    {
        Motion *motion  = *(queue.begin());
        queue.erase(queue.begin());

        if (motion->costApx_ > (1 + epsilon_) * motion->costLb_)
        {
            Motion *potential_parent = getMotion(lowerBoundGraph_.getShortestPathParent(motion->id_));
            if (checkMotion(potential_parent, motion))
            {
                double delta = lazilyUpdateApxParent(motion, potential_parent);
                updateChildCostsApx(motion, delta);
            }
            else
            {
                affected.clear();

                lowerBoundGraph_.removeEdge(potential_parent->id_, motion->id_, true, affected);

                for (iter = affected.begin(); iter != affected.end(); ++iter)
                {
                    Motion *affected = getMotion(*iter);
                    Lb_queue_iter lb_queue_iter = queue.find(affected);
                    if (lb_queue_iter != queue.end())
                    {
                        queue.erase(lb_queue_iter);
                        affected->costLb_ = lowerBoundGraph_.getShortestPathCost(affected->id_);
                        if (affected->costApx_ > (1 + epsilon_) * affected->costLb_)
                            queue.insert(affected);
                    }
                    else
                    {
                        affected->costLb_ = lowerBoundGraph_.getShortestPathCost(affected->id_);
                    }
                }

                motion->costLb_ = lowerBoundGraph_.getShortestPathCost(motion->id_);
                if (motion->costApx_ > (1 + epsilon_) * motion->costLb_)
                    queue.insert(motion);

                // optimization - we can remove the opposite edge
                lowerBoundGraph_.removeEdge(motion->id_, potential_parent->id_, false, affected);
            }
        }
    }

    return;
}
Пример #12
0
//**************************************************************************************************************************pointCloudcallback()
void PSMpositionNode::pointCloudcallback(const sensor_msgs::PointCloud2& pcloud)
{

    ROS_DEBUG("Received PointCloud");
    pclCount_++;
    struct timeval start, end;
    gettimeofday(&start, NULL);

    // **** if this is the first scan, initialize and leave the function here

    if (!init_)
    {
        init_ = initializeLasermsgs(pcloud);
        if (init_) ROS_INFO("Lasermsgs initialized");
        return;
    }
    hline.header = pcloud.header;
    hline.header.frame_id  = "hkinectline";

    pcl::fromROSMsg (pcloud, cloud);
    std::vector <depthPoint> verticalLine;

    //get the horizontal central line
    for (int i=0; i<width_; i++)
    {

        if (isnan(cloud.points[begin_+i].x) || isnan(cloud.points[begin_+i].y) || isnan(cloud.points[begin_+i].z))
        {
            hline.ranges[width_-1-i] = 0;
        }
        else
        {
            hline.ranges[width_-1-i] = sqrt(pow(cloud.points[begin_+i].x,2)+pow(cloud.points[begin_+i].y,2)+pow(cloud.points[begin_+i].z,2));
        }
    }
//ROS_INFO("Distance to wall: %f",hline.ranges[width_/2]);
    //get the vertical central line
    for (int i=0; i<height_; i++)
    {

        //for altitude estimation
        XYpoint.x = cloud.points[(i+0.5)*width_].z ;
        XYpoint.y = cloud.points[(i+0.5)*width_].y ;

        if (isnan(cloud.points[(i+0.5)*width_].x) || isnan(cloud.points[(i+0.5)*width_].y) || isnan(cloud.points[(i+0.5)*width_].z))
        {
            vline.ranges[i] = 0;
        }
        else
        {
            verticalLine.push_back(XYpoint);
            //vline.ranges[i] = sqrt(pow(cloud.points[(i+0.5)*width_].x,2)+pow(cloud.points[(i+0.5)*width_].y,2)+pow(cloud.points[(i+0.5)*width_].z,2));
        }
    }


    if(smMethod==1)getMotion(hline, &verticalLine);
    if(smMethod==2)getMotion_can(hline, &verticalLine);

    gettimeofday(&end, NULL);
    double dur = ((end.tv_sec   * 1000000 + end.tv_usec  ) -
                  (start.tv_sec * 1000000 + start.tv_usec)) / 1000.0;
    totalDuration_ += dur;
    double ave = totalDuration_/pclCount_;

    ROS_INFO("dur:\t %.3f ms \t ave:\t %.3f ms", dur, ave);

}
Пример #13
0
//get the action target point in the images reference frame
bool VisuoThread::getTarget(Value &type, Bottle &options)
{
    bool ok=false;

    Bottle &bNewTarget=options.addList();
    bNewTarget.addString("target");

    Bottle &bTarget=bNewTarget.addList();
    Bottle &bNewStereo=bTarget.addList();
    bNewStereo.addString("stereo");

    Bottle &bStereo=bNewStereo.addList();

    if(type.isList())
    {
        Bottle *list=type.asList();
        if(list->size()>2)
        {
            if(list->get(0).asString()=="right")
            {
                bStereo.addDouble(0.0);
                bStereo.addDouble(0.0);
                bStereo.addDouble(list->get(1).asDouble());
                bStereo.addDouble(list->get(2).asDouble());
                
            }
            else if(list->get(0).asString()=="left")
            {
                bStereo.addDouble(list->get(1).asDouble());
                bStereo.addDouble(list->get(2).asDouble());
                bStereo.addDouble(0.0);
                bStereo.addDouble(0.0);
            }
            else if(list->get(0).asString()=="cartesian")
            {
                Bottle &bNewCartesian=bTarget.addList();
                bNewCartesian.addString("cartesian");

                Bottle &bCartesian=bNewCartesian.addList();

                for(int i=1; i<list->size(); i++)
                    bCartesian.addDouble(list->get(i).asDouble());
            }
            else
            {
                Bottle &bNewCartesian=bTarget.addList();
                bNewCartesian.addString("cartesian");

                Bottle &bCartesian=bNewCartesian.addList();

                for(int i=0; i<list->size(); i++)
                    bCartesian.addDouble(list->get(i).asDouble());
            }

            ok=true;
        }
        else if(list->size()==2)
        {
            bStereo.addDouble(list->get(0).asDouble());
            bStereo.addDouble(list->get(1).asDouble());
            bStereo.addDouble(0.0);
            bStereo.addDouble(0.0);

            ok=true;
        }
    }
    else switch(type.asVocab())
    {
        case PARAM_FIXATION:
        {
            ok=getFixation(bStereo);
            break;
        }
        case PARAM_MOTION:
        {
            ok=getMotion(bStereo);
            break;
        }
        case PARAM_TRACK:
        {
            ok=getTrack(bStereo);
            break;
        }
        case PARAM_RAW:
        {
            ok=getRaw(bStereo);
            break;
        }

        default:
        {
            Bottle &bName=bTarget.addList();
            bName.addString("name");
            bName.addString(type.asString().c_str());

            getObject(type.asString().c_str(),bStereo);
            break;
        }
    }


    return ok;
}