Example #1
0
        public VFHFollower(SensorPose sickPose, SensorPose hokuyoPose, double angularResolution, bool useHokuyo)
        {
            pose = null;

            this.useHokuyo = useHokuyo;

            this.sickPose = sickPose;
            sickDCM = Matrix4.FromPose(this.sickPose);

            if (this.useHokuyo)
            {
                this.hokuyoPose = hokuyoPose;
                hokuyoDCM = Matrix4.FromPose(this.hokuyoPose);
            }

            sickHistory = new List<Vector2[]>();
            hokuyoHistory = new List<Vector2[]>();

            this.angularResolution = angularResolution;
            numBuckets = (int) Math.Round(360 / this.angularResolution);
            trapped = false;
            kThreshold = (int) Math.Round(160 / this.angularResolution);

            rHistogram = new Double[numBuckets];
        }
 public void SetSensorPose(SensorPose laserPose, bool forCamera)
 {
     laserToRobot = Matrix4.FromPose(laserPose);
     if (forCamera)
         laserToRobot[2, 3] = 0;
     //laserToRobot[0, 2] = laserPose.x;
     //laserToRobot[1, 2] = laserPose.y;
     //laserToRobot[2, 2] = 0;
     //laserToRobot[3, 3] = 1.0;
 }
        double thresholdHeight; // above 5 cm

        #endregion Fields

        #region Constructors

        public LidarTargetDetector(double thresholdHeight, SensorPose laserPose, bool forCamera)
        {
            this.thresholdHeight = thresholdHeight;
            laserToRobot = Matrix4.FromPose(laserPose);
            if (forCamera)
                laserToRobot[2, 3] = 0;
            //laserToRobot[0, 2] = laserPose.x;
            //laserToRobot[1, 2] = laserPose.y;
            //laserToRobot[2, 2] = laserPose.z;
            //laserToRobot[3, 3] = 1.0;
        }
Example #4
0
        public Vector2[] TransformToGlobal(ILidarScan<ILidar2DPoint> scan, Matrix4 sensorDcm)
        {
            Vector2[] globalScan = new Vector2[scan.Points.Count];
            Vector4 point;

            Matrix4 transformDcm = Matrix4.FromPose(pose) * sensorDcm;

            int n = scan.Points.Count;

            for (int i = 0; i < n; i++)
            {
                point = transformDcm * scan.Points[i].RThetaPoint.ToVector4();
                if (point.Z >= 0.05)
                    globalScan[i] = new Vector2(point.X, point.Y);
            }

            return globalScan;
        }
        public static List<Vector2> ExtractGlobalFeature(ILidarScan<ILidar2DPoint> scan, SensorPose sensorPose, RobotPose currentPose, double wheelRadius)
        {
            Stopwatch sw = new Stopwatch();
            sw.Start();
            List<Vector2> features = new List<Vector2>();
            List<ILidar2DPoint> filteredScan = new List<ILidar2DPoint>();
            //foreach (ILidar2DPoint pt in scan.Points)
            for (int i = 0; i < scan.Points.Count; i++)
            {
                if (scan.Points[i].RThetaPoint.R > 0.20)
                    filteredScan.Add(scan.Points[i]);
            }

            Matrix4 laser2robotDCM = new Matrix4(1, 0, 0, 0,
                                                 0, Math.Cos(sensorPose.pitch), Math.Sin(sensorPose.pitch), 0,
                                                 0, -Math.Sin(sensorPose.pitch), Math.Cos(sensorPose.pitch), sensorPose.z,
                                                 0, 0, 0, 1);

            int neighbor = 3;
            for (int i = neighbor; i < filteredScan.Count - neighbor; i++)
            {
                // AGAIN, these codes are written respect to ENU coordinate frame
                double currentX = -filteredScan[i].RThetaPoint.R * Math.Sin(filteredScan[i].RThetaPoint.theta) - sensorPose.y;
                double currentY = filteredScan[i].RThetaPoint.R * Math.Cos(filteredScan[i].RThetaPoint.theta) + sensorPose.x;
                double lastX = -filteredScan[i - neighbor].RThetaPoint.R * Math.Sin(filteredScan[i - neighbor].RThetaPoint.theta) - sensorPose.y;
                double lastY = filteredScan[i - neighbor].RThetaPoint.R * Math.Cos(filteredScan[i - neighbor].RThetaPoint.theta) + sensorPose.x;
                double nextX = -filteredScan[i + neighbor].RThetaPoint.R * Math.Sin(filteredScan[i + neighbor].RThetaPoint.theta) - sensorPose.y;
                double nextY = filteredScan[i + neighbor].RThetaPoint.R * Math.Cos(filteredScan[i + neighbor].RThetaPoint.theta) + sensorPose.x;

                Matrix localPt = new Matrix(4, 1), lastLocalPt = new Matrix(4, 1), nextLocalPt = new Matrix(4, 1);
                localPt[0, 0] = currentX; localPt[1, 0] = currentY; localPt[3, 0] = 1;
                lastLocalPt[0, 0] = lastX; lastLocalPt[1, 0] = lastY; lastLocalPt[3, 0] = 1;
                nextLocalPt[0, 0] = nextX; nextLocalPt[1, 0] = nextY; nextLocalPt[3, 0] = 1;
                Vector4 currentPt = laser2robotDCM * localPt;
                Vector4 lastPt = laser2robotDCM * lastLocalPt;
                Vector4 nextPt = laser2robotDCM * nextLocalPt;

                // check if the z position of the laser return is higher than 10 cm of current z-state
                //if (currentPt.Z > currentPose.z + 0.1)
                //    features.Add(new Vector2(currentPt.X, currentPt.Y));

                // height comparison
                Vector2 neighborPt = FindNeighborToCompare(filteredScan, sensorPose, i, 0.1);
                Matrix neighbotPtM = new Matrix(4, 1);
                neighbotPtM[0, 0] = neighborPt.X; neighbotPtM[1, 0] = neighborPt.Y; neighbotPtM[3, 0] = 1;
                Vector4 neighborPtV = laser2robotDCM * neighbotPtM;

                double yaw = currentPose.yaw - Math.PI / 2;
                double heightDiff = neighborPtV.Z - currentPt.Z;
                if (heightDiff > 0.1)
                {
                    features.Add(new Vector2(neighborPtV.X * Math.Cos(yaw) - neighborPtV.Y * Math.Sin(yaw) + currentPose.x,
                                            neighborPtV.X * Math.Sin(yaw) + neighborPtV.Y * Math.Cos(yaw) + currentPose.y));
                }
                else if (heightDiff < -0.10)
                {
                    features.Add(new Vector2(currentPt.X * Math.Cos(yaw) - currentPt.Y * Math.Sin(yaw) + currentPose.x,
                                            currentPt.X * Math.Sin(yaw) + currentPt.Y * Math.Cos(yaw) + currentPose.y));
                }

            }
            Console.WriteLine("Jump(?) Extraction Time:" + sw.ElapsedMilliseconds);
            return features;
        }
Example #6
0
        public static Matrix4 YPR(double yaw, double pitch, double roll)
        {
            double cy = Math.Cos(yaw), sy = Math.Sin(yaw);
            double cp = Math.Cos(pitch), sp = Math.Sin(pitch);
            double cr = Math.Cos(roll), sr = Math.Sin(roll);

            //Matrix4 rz = new Matrix4(
            //   cy, -sy, 0, 0,
            //   sy, cy, 0, 0,
            //    0, 0, 1, 0,
            //    0, 0, 0, 1
            //  );

            //Matrix4 ry = new Matrix4(
            //   cp, 0, sp, 0,
            //    0, 1, 0, 0,
            //   -sp, 0, cp, 0,
            //    0, 0, 0, 1
            //  );

            //Matrix4 rx = new Matrix4(
            //  1, 0, 0, 0,
            //  0, cr, -sr, 0,
            //  0, sr, cr, 0,
            //  0, 0, 0, 1
            //  );

            Matrix4 rz = new Matrix4(
                 cy, sy, 0, 0,
                 -sy, cy, 0, 0,
                    0, 0, 1, 0,
                    0, 0, 0, 1
                );

            Matrix4 ry = new Matrix4(
                 cp, 0, -sp, 0,
                    0, 1, 0, 0,
                 sp, 0, cp, 0,
                    0, 0, 0, 1
                );

            Matrix4 rx = new Matrix4(
                1, 0, 0, 0,
                0, cr, sr, 0,
                0, -sr, cr, 0,
                0, 0, 0, 1
                );

            return rx * ry * rz;
        }
Example #7
0
        public static Matrix4 TransformationMatrix(double dx, double dy, double dz, double ez, double ey, double ex, bool useDCMTranspose)
        {
            Matrix4 T = new Matrix4();
            Matrix3 dcm = Matrix3.DCM(ez, ey, ex);

            //Assign the full transformation matrix
            for (int i = 0; i < 3; i++)
            {
                for (int j = 0; j < 3; j++)
                {
                    //Test if using the dcm transpose
                    if (useDCMTranspose)
                        T[i, j] = dcm[j, i];
                    else
                        T[i, j] = dcm[i, j];
                }
            }

            //Assign the translation
            T[0, 3] = dx;
            T[1, 3] = dy;
            T[2, 3] = dz;
            T[3, 3] = 1.0;

            return T;
        }
Example #8
0
        public static Matrix4 FromSubmatrix(
			Matrix2 A, Matrix2 B,
			Matrix2 C, Matrix2 D)
        {
            Matrix4 ret = new Matrix4();

            for (int i = 0; i < 2; i++)
            {
                for (int j = 0; j < 2; j++)
                {
                    ret[i, j] = A[i, j];
                    ret[i + 2, j] = C[i, j];
                    ret[i, j + 2] = B[i, j];
                    ret[i + 2, j + 2] = D[i, j];
                }
            }

            return ret;
        }
Example #9
0
        public static Matrix4 operator *(Matrix4 l, Matrix4 r)
        {
            // [      ld0*rd0+ld1*rd4+ld2*rd8+ld3*rd12,      ld0*rd1+ld1*rd5+ld2*rd9+ld3*rd13,     ld0*rd2+ld1*rd6+ld2*rd10+ld3*rd14,     ld0*rd3+ld1*rd7+ld2*rd11+ld3*rd15]
            // [      ld4*rd0+ld5*rd4+ld6*rd8+ld7*rd12,      ld4*rd1+ld5*rd5+ld6*rd9+ld7*rd13,     ld4*rd2+ld5*rd6+ld6*rd10+ld7*rd14,     ld4*rd3+ld5*rd7+ld6*rd11+ld7*rd15]
            // [    ld8*rd0+ld9*rd4+ld10*rd8+ld11*rd12,    ld8*rd1+ld9*rd5+ld10*rd9+ld11*rd13,   ld8*rd2+ld9*rd6+ld10*rd10+ld11*rd14,   ld8*rd3+ld9*rd7+ld10*rd11+ld11*rd15]
            // [  ld12*rd0+ld13*rd4+ld14*rd8+ld15*rd12,  ld12*rd1+ld13*rd5+ld14*rd9+ld15*rd13, ld12*rd2+ld13*rd6+ld14*rd10+ld15*rd14, ld12*rd3+ld13*rd7+ld14*rd11+ld15*rd15]
            Matrix4 ret = new Matrix4();
            for (int i = 0; i < 4; i++)
            {
                for (int j = 0; j < 4; j++)
                {
                    ret[i, j] = l[i, 0] * r[0, j] + l[i, 1] * r[1, j] + l[i, 2] * r[2, j] + l[i, 3] * r[3, j];
                }
            }

            return ret;
        }