示例#1
0
 /// <summary>
 /// Dispose of any resources.
 /// </summary>
 public override void Dispose()
 {
     if (handle.Handle != IntPtr.Zero)
     {
         ISAM2Lib.deletenavigator(handle);
         handle = new HandleRef(this, IntPtr.Zero);
     }
 }
示例#2
0
        /// <summary>
        /// Get the map estimate.
        /// </summary>
        /// <param name="plmodel">Side model that contains the covariances of the
        /// pose-landmark pairs projected into the measurement space.
        /// Takes the form E = JSJ^T + R, where
        /// J is the measurement jacobian in both the pose and landmark directions,
        /// S is the joint covariance of the best estimate current pose and the landmark and
        /// R is the measurement noise.
        /// </param>
        /// <returns>Map estimate as points with certain covariance (gaussians).</returns>
        private unsafe IndexedMap GetMapModel(out IndexedMap plmodel)
        {
            int        length;
            Gaussian   component;
            IndexedMap mapmodel = new IndexedMap(3);

            plmodel = new IndexedMap(3);

            double *ptrmapmodel = (double *)ISAM2Lib.getmapmodel(handle, out length);
            double *ptrmapcov   = (double *)ISAM2Lib.getmapcovariances(handle, out length);
            double *ptrplcov    = (double *)ISAM2Lib.getplcovariances(handle, out length);

            // main model: mean and covariance
            for (int i = 0, k = 0, h = 0; i < length; i++, k += 3, h += 9)
            {
                double[]   mean       = new double[3];
                double[][] covariance = { new double[3], new double[3], new double[3] };

                mean[0] = ptrmapmodel[k + 0];
                mean[1] = ptrmapmodel[k + 1];
                mean[2] = ptrmapmodel[k + 2];

                covariance[0][0] = ptrmapcov[h + 0];
                covariance[0][1] = ptrmapcov[h + 1];
                covariance[0][2] = ptrmapcov[h + 2];
                covariance[1][0] = ptrmapcov[h + 3];
                covariance[1][1] = ptrmapcov[h + 4];
                covariance[1][2] = ptrmapcov[h + 5];
                covariance[2][0] = ptrmapcov[h + 6];
                covariance[2][1] = ptrmapcov[h + 7];
                covariance[2][2] = ptrmapcov[h + 8];

                component = new Gaussian(mean, covariance, 1.0);
                mapmodel.Add(component);
            }

            // side model: covariance projected into measurement space
            // includes pose uncertainty and measurement noise
            for (int i = 0, h = 0; i < length; i++, h += 9)
            {
                double[][] covariance = { new double[3], new double[3], new double[3] };

                covariance[0][0] = ptrplcov[h + 0];
                covariance[0][1] = ptrplcov[h + 1];
                covariance[0][2] = ptrplcov[h + 2];
                covariance[1][0] = ptrplcov[h + 3];
                covariance[1][1] = ptrplcov[h + 4];
                covariance[1][2] = ptrplcov[h + 5];
                covariance[2][0] = ptrplcov[h + 6];
                covariance[2][1] = ptrplcov[h + 7];
                covariance[2][2] = ptrplcov[h + 8];

                component = new Gaussian(new double[3], covariance, 1.0);
                plmodel.Add(component);
            }

            return(mapmodel);
        }
示例#3
0
        /// <summary>
        /// Update the solver estimates from unmanaged code.
        /// </summary>
        /// <param name="newstate">Updated state after odometry as [tx, ty, tz, qw, qx, qy, qz].</param>
        /// <param name="measurements">Vectorized measurement list as [x1, y1, z1, x2, y2, z2, ...].</param>
        /// <param name="labels">Data association labels, one per measurement.</param>
        /// <param name="nmeasurements">Number of measurements.</param>
        /// <returns>Error code: 0 = ok, 1 = divergence, 2 = duplicate var, -1 = general error.</returns>
        private unsafe int Update(double[] newstate, double[] measurements, int[] labels, int nmeasurements)
        {
            int retvalue = -1;

            fixed(double *ptrNewState = newstate)
            {
                fixed(double *ptrMeasurements = measurements)
                {
                    fixed(int *ptrLabels = labels)
                    {
                        retvalue = ISAM2Lib.update(handle, (IntPtr)ptrNewState, (IntPtr)ptrMeasurements, (IntPtr)ptrLabels, nmeasurements, OnlyMapping);
                    }
                }
            }

            return(retvalue);
        }
示例#4
0
        /// <summary>
        /// Get the trajectory estimate from unmanaged code.
        /// </summary>
        /// <returns>Trajectory estimate.</returns>
        private unsafe List <double[]> GetTrajectory()
        {
            int             length;
            List <double[]> trajectory = new List <double[]>();

            double *ptrtrajectory = (double *)ISAM2Lib.gettrajectory(handle, out length);

            for (int i = 0, k = 0; i < length; i++)
            {
                double[] point = new double[StateSize];

                for (int h = 0; h < StateSize; h++, k++)
                {
                    point[h] = ptrtrajectory[k];
                }

                trajectory.Add(point);
            }

            return(trajectory);
        }
示例#5
0
        /// <summary>
        /// Construct a new ISAM2Navigator from unmanaged code.
        /// </summary>
        /// <param name="initPose">Initial pose.</param>
        /// <param name="measurementNoise">Vectorized measurement noise.</param>
        /// <param name="motionNoise">Vectorized motion noise.</param>
        /// <param name="mparams">Measurer configuration parameters.</param>
        /// <returns>New navigator.</returns>
        private unsafe HandleRef NewNavigator(double[] initPose, double[] measurementNoise,
                                              double[] motionNoise, double[] mparams)
        {
            IntPtr ptr = IntPtr.Zero;

            fixed(double *ptrInitPose = initPose)
            {
                fixed(double *ptrMeasurementNoise = measurementNoise)
                {
                    fixed(double *ptrMotionNoise = motionNoise)
                    {
                        fixed(double *ptrMParams = mparams)
                        {
                            ptr = ISAM2Lib.newnavigator((IntPtr)ptrInitPose, (IntPtr)ptrMeasurementNoise,
                                                        (IntPtr)ptrMotionNoise, (IntPtr)ptrMParams);
                        }
                    }
                }
            }

            return(new HandleRef(this, ptr));
        }