public static void Test()
        {
            CoordRotTransConversion crtc = new CoordRotTransConversion();
            Random rand = new Random();
            CvMat  cov  = new CvMat(4, 4, MatrixType.F64C1);

            cov.Zero();
            cov[0, 3] = rand.NextDouble() * 200 - 500;
            cov[1, 3] = rand.NextDouble() * 200 - 500;
            cov[2, 3] = rand.NextDouble() * 200 - 500;
            cov[3, 3] = 1.0;
            CvMat rotateConversion;

            cov.GetSubRect(out rotateConversion, new CvRect(0, 0, 3, 3));
            CvMat rotVector = new CvMat(1, 3, MatrixType.F64C1);

            rotVector[0, 0] = rand.NextDouble() * 10 - 5;
            rotVector[0, 1] = rand.NextDouble() * 10 - 5;
            rotVector[0, 2] = rand.NextDouble() * 10 - 5;
            Cv.Rodrigues2(rotVector, rotateConversion);

            for (int i = 0; i < 100000; i++)
            {
                CvPoint3D64f from    = new CvPoint3D64f(rand.NextDouble() * rand.NextDouble() * rand.NextDouble() * 200 - 500, rand.NextDouble() * 200 - 500, rand.NextDouble() * 200 - 500);
                CvMat        fromMat = new CvMat(4, 1, MatrixType.F64C1);
                CvEx.FillCvMat(fromMat, new double[] { from.X, from.Y, from.Z, 1.0 });
                CvMat        toMat = cov * fromMat;
                CvPoint3D64f to    = new CvPoint3D64f(toMat[0, 0], toMat[0, 1], toMat[0, 2]);
                crtc.PutPoint(from, to, 1.0);
            }
            CvMat ret = crtc.Solve();
            Func <CvMat, CvMat, string> show = (i, o) =>
            {
                StringBuilder str = new StringBuilder();
                str.AppendFormat("{0} = {1} / {2}\n", "11", i[0, 0].ToString("0.000"), o[0, 0].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "12", i[0, 1].ToString("0.000"), o[0, 1].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "13", i[0, 2].ToString("0.000"), o[0, 2].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "14", i[0, 3].ToString("0.000"), o[0, 3].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "21", i[1, 0].ToString("0.000"), o[1, 0].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "22", i[1, 1].ToString("0.000"), o[1, 1].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "23", i[1, 2].ToString("0.000"), o[1, 2].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "24", i[1, 3].ToString("0.000"), o[1, 3].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "31", i[2, 0].ToString("0.000"), o[2, 0].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "32", i[2, 1].ToString("0.000"), o[2, 1].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "33", i[2, 2].ToString("0.000"), o[2, 2].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "34", i[2, 3].ToString("0.000"), o[2, 3].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "41", i[3, 0].ToString("0.000"), o[3, 0].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "42", i[3, 1].ToString("0.000"), o[3, 1].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "43", i[3, 2].ToString("0.000"), o[3, 2].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "44", i[3, 3].ToString("0.000"), o[3, 3].ToString("0.000"));
                return(str.ToString());
            };

            MessageBox.Show(show(cov, ret));
        }
        public static void Test()
        {
            CoordRotTransConversion crtc = new CoordRotTransConversion();
            Random rand = new Random();
            CvMat cov = new CvMat(4, 4, MatrixType.F64C1);
            cov.Zero();
            cov[0, 3] = rand.NextDouble() * 200 - 500;
            cov[1, 3] = rand.NextDouble() * 200 - 500;
            cov[2, 3] = rand.NextDouble() * 200 - 500;
            cov[3, 3] = 1.0;
            CvMat rotateConversion;
            cov.GetSubRect(out rotateConversion, new CvRect(0, 0, 3, 3));
            CvMat rotVector = new CvMat(1, 3, MatrixType.F64C1);
            rotVector[0, 0] = rand.NextDouble() * 10 - 5;
            rotVector[0, 1] = rand.NextDouble() * 10 - 5;
            rotVector[0, 2] = rand.NextDouble() * 10 - 5;
            Cv.Rodrigues2(rotVector, rotateConversion);

            for (int i = 0; i < 100000; i++)
            {
                CvPoint3D64f from = new CvPoint3D64f(rand.NextDouble() * rand.NextDouble() * rand.NextDouble() * 200 - 500, rand.NextDouble() * 200 - 500, rand.NextDouble() * 200 - 500);
                CvMat fromMat = new CvMat(4, 1, MatrixType.F64C1);
                CvEx.FillCvMat(fromMat, new double[] { from.X, from.Y, from.Z, 1.0 });
                CvMat toMat = cov * fromMat;
                CvPoint3D64f to = new CvPoint3D64f(toMat[0, 0], toMat[0, 1], toMat[0, 2]);
                crtc.PutPoint(from, to, 1.0);
            }
            CvMat ret = crtc.Solve();
            Func<CvMat, CvMat, string> show = (i, o) =>
            {
                StringBuilder str = new StringBuilder();
                str.AppendFormat("{0} = {1} / {2}\n", "11", i[0, 0].ToString("0.000"), o[0, 0].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "12", i[0, 1].ToString("0.000"), o[0, 1].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "13", i[0, 2].ToString("0.000"), o[0, 2].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "14", i[0, 3].ToString("0.000"), o[0, 3].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "21", i[1, 0].ToString("0.000"), o[1, 0].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "22", i[1, 1].ToString("0.000"), o[1, 1].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "23", i[1, 2].ToString("0.000"), o[1, 2].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "24", i[1, 3].ToString("0.000"), o[1, 3].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "31", i[2, 0].ToString("0.000"), o[2, 0].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "32", i[2, 1].ToString("0.000"), o[2, 1].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "33", i[2, 2].ToString("0.000"), o[2, 2].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "34", i[2, 3].ToString("0.000"), o[2, 3].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "41", i[3, 0].ToString("0.000"), o[3, 0].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "42", i[3, 1].ToString("0.000"), o[3, 1].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "43", i[3, 2].ToString("0.000"), o[3, 2].ToString("0.000"));
                str.AppendFormat("{0} = {1} / {2}\n", "44", i[3, 3].ToString("0.000"), o[3, 3].ToString("0.000"));
                return str.ToString();
            };
            MessageBox.Show(show(cov, ret));
        }
        /// <summary>
        /// あるフレームにおける座標変換行列を骨格情報から計算する
        /// </summary>
        /// <param name="frame"></param>
        /// <param name="convList"></param>
        /// <param name="bodies"></param>
        /// <returns></returns>
        public static List <CvMat> GetConvMatrixFromBoneFrame(Frame frame, List <CvMat> convList, List <SerializableBody> bodies)
        {
            if (bodies.Count() != frame.recordNum)
            {
                System.Windows.MessageBox.Show("ユーザが選択されていないレコードがあります");
                return(convList);
            }

            bool[] validFlags = frame.GetValidFlags();

            for (int j = 1; j < frame.recordNum; j++)
            {
                Dictionary <JointType, Joint> joint1 = Utility.GetValidJoints(bodies[0].Joints);
                Dictionary <JointType, Joint> joint2 = Utility.GetValidJoints(bodies[j].Joints);
                if (validFlags[0] && validFlags[j] == false)
                {
                    continue;
                }
                ICoordConversion3D crtc = new CoordRotTransConversion();
                foreach (JointType jointType in Enum.GetValues(typeof(JointType)))
                {
                    if (!joint1.ContainsKey(jointType))
                    {
                        continue;
                    }
                    if (!joint2.ContainsKey(jointType))
                    {
                        continue;
                    }
                    CvPoint3D64f from   = joint2[jointType].Position.ToCvPoint3D();
                    CvPoint3D64f target = CvEx.ConvertPoint3D(joint1[jointType].Position.ToCvPoint3D(), convList[0]);
                    // IsOriginlJointValid相当の処理を入れるかどうか
                    crtc.PutPoint(from, target, 1);
                }
                convList[j] = crtc.Solve();
            }
            return(convList);
        }
        public CvMat CalculateTransform(int targetModelIndex, bool updateInternalModelTransform, double randomSamplingRatio)
        {
            if (targetModelIndex < 0 || targetModelIndex >= _flannModels.Count)
                throw new ArgumentOutOfRangeException("targetModelIndex");
            CoordRotTransConversion coordConverter = new CoordRotTransConversion();
            //CoordConvertSpring coordConverter = new CoordConvertSpring(_modelTransforms[targetModelIndex]);
            //foreach (var point in dataPointListInWorldCoordinate) {
            List<Tuple<CvPoint3D64f, CvColor>> tuples = _flannModels[targetModelIndex].ModelPoints;
            if (randomSamplingRatio < 1)
            {
                Random rand = new Random();
                tuples = tuples.Where(x => rand.NextDouble() < randomSamplingRatio).ToList();
            }
            CvMat targetTransform = _modelTransforms[targetModelIndex];

            List<CvMat> inverseTransforms = new List<CvMat>();
            foreach (CvMat transform in _modelTransforms)
            {
                CvMat inv = CvEx.InitCvMat(transform);
                transform.Invert(inv);
                inverseTransforms.Add(inv);
            }
            float searchDistanceSq = this.SearchDistance * this.SearchDistance;
            Parallel.ForEach(tuples, tuple =>
            {
                CvPoint3D64f point = tuple.Item1;
                CvColor color = tuple.Item2;
                //foreach (var point in points) {
                CvPoint3D64f worldPoint = CvEx.ConvertPoint3D(point, targetTransform);
                int minModelIndex = -1;
                int minPointIndex = -1;
                float minDistanceSq = float.MaxValue;
                for (int modelIndex = 0; modelIndex < _flannModels.Count; modelIndex++)
                {
                    if (modelIndex == targetModelIndex)
                        continue;
                    CvPoint3D64f inversePoint = CvEx.ConvertPoint3D(worldPoint, inverseTransforms[modelIndex]);
                    int[] indices;
                    float[] distances;
                    _flannModels[modelIndex].KnnSearch(inversePoint, color, out indices, out distances, 1);
                    if (indices.Length >= 1)
                    {
                        float distanceSq = distances[0];
                        if (distanceSq <= searchDistanceSq)
                        {
                            if (distanceSq < minDistanceSq)
                            {
                                minModelIndex = modelIndex;
                                minPointIndex = indices[0];
                                minDistanceSq = distanceSq;
                            }
                        }
                    }
                }
                if (minModelIndex != -1)
                {
                    Tuple<CvPoint3D64f, CvColor> bestModelPoint = _flannModels[minModelIndex].ModelPoints[minPointIndex];
                    double weightTo = 1.0 / (Math.Abs(bestModelPoint.Item1.Z - 1500 / 1000f) + 5000 / 1000f);
                    double weightFrom = 1.0 / (Math.Abs(point.Z - 1500 / 1000f) + 5000 / 1000f);
                    //weightFrom = weightTo = 1;
                    double weight = _weightFromDistanceSq(minDistanceSq) * weightFrom * weightTo;
                    CvPoint3D64f from = CvEx.ConvertPoint3D(point, targetTransform);
                    CvPoint3D64f to = CvEx.ConvertPoint3D(bestModelPoint.Item1, _modelTransforms[minModelIndex]);

                    coordConverter.PutPoint(from, to, weight);
                }
            });
            CvMat ret = coordConverter.Solve() * targetTransform;
            if (updateInternalModelTransform)
            {
                _modelTransforms[targetModelIndex] = ret.Clone();
            }
            return ret;
        }
Example #5
0
        public CvMat CalculateTransform(int targetModelIndex, bool updateInternalModelTransform, double randomSamplingRatio)
        {
            if (targetModelIndex < 0 || targetModelIndex >= _flannModels.Count)
            {
                throw new ArgumentOutOfRangeException("targetModelIndex");
            }
            CoordRotTransConversion coordConverter = new CoordRotTransConversion();
            //CoordConvertSpring coordConverter = new CoordConvertSpring(_modelTransforms[targetModelIndex]);
            //foreach (var point in dataPointListInWorldCoordinate) {
            List <Tuple <CvPoint3D64f, CvColor> > tuples = _flannModels[targetModelIndex].ModelPoints;

            if (randomSamplingRatio < 1)
            {
                Random rand = new Random();
                tuples = tuples.Where(x => rand.NextDouble() < randomSamplingRatio).ToList();
            }
            CvMat targetTransform = _modelTransforms[targetModelIndex];

            List <CvMat> inverseTransforms = new List <CvMat>();

            foreach (CvMat transform in _modelTransforms)
            {
                CvMat inv = CvEx.InitCvMat(transform);
                transform.Invert(inv);
                inverseTransforms.Add(inv);
            }
            float searchDistanceSq = this.SearchDistance * this.SearchDistance;

            Parallel.ForEach(tuples, tuple =>
            {
                CvPoint3D64f point = tuple.Item1;
                CvColor color      = tuple.Item2;
                //foreach (var point in points) {
                CvPoint3D64f worldPoint = CvEx.ConvertPoint3D(point, targetTransform);
                int minModelIndex       = -1;
                int minPointIndex       = -1;
                float minDistanceSq     = float.MaxValue;
                for (int modelIndex = 0; modelIndex < _flannModels.Count; modelIndex++)
                {
                    if (modelIndex == targetModelIndex)
                    {
                        continue;
                    }
                    CvPoint3D64f inversePoint = CvEx.ConvertPoint3D(worldPoint, inverseTransforms[modelIndex]);
                    int[] indices;
                    float[] distances;
                    _flannModels[modelIndex].KnnSearch(inversePoint, color, out indices, out distances, 1);
                    if (indices.Length >= 1)
                    {
                        float distanceSq = distances[0];
                        if (distanceSq <= searchDistanceSq)
                        {
                            if (distanceSq < minDistanceSq)
                            {
                                minModelIndex = modelIndex;
                                minPointIndex = indices[0];
                                minDistanceSq = distanceSq;
                            }
                        }
                    }
                }
                if (minModelIndex != -1)
                {
                    Tuple <CvPoint3D64f, CvColor> bestModelPoint = _flannModels[minModelIndex].ModelPoints[minPointIndex];
                    double weightTo   = 1.0 / (Math.Abs(bestModelPoint.Item1.Z - 1500 / 1000f) + 5000 / 1000f);
                    double weightFrom = 1.0 / (Math.Abs(point.Z - 1500 / 1000f) + 5000 / 1000f);
                    //weightFrom = weightTo = 1;
                    double weight     = _weightFromDistanceSq(minDistanceSq) * weightFrom * weightTo;
                    CvPoint3D64f from = CvEx.ConvertPoint3D(point, targetTransform);
                    CvPoint3D64f to   = CvEx.ConvertPoint3D(bestModelPoint.Item1, _modelTransforms[minModelIndex]);

                    coordConverter.PutPoint(from, to, weight);
                }
            });
            CvMat ret = coordConverter.Solve() * targetTransform;

            if (updateInternalModelTransform)
            {
                _modelTransforms[targetModelIndex] = ret.Clone();
            }
            return(ret);
        }
        /// <summary>
        /// あるフレームにおける座標変換行列を骨格情報から計算する
        /// </summary>
        /// <param name="frame"></param>
        /// <param name="convList"></param>
        /// <param name="bodies"></param>
        /// <returns></returns>
        public static List<CvMat> GetConvMatrixFromBoneFrame(Frame frame, List<CvMat> convList, List<SerializableBody> bodies)
        {
            if ( bodies.Count() != frame.recordNum )
            {
                System.Windows.MessageBox.Show("ユーザが選択されていないレコードがあります");
                return convList;
            }

            bool[] validFlags = frame.GetValidFlags();

            for (int j = 1; j < frame.recordNum; j++)
            {
                Dictionary<JointType, Joint> joint1 = Utility.GetValidJoints(bodies[0].Joints);
                Dictionary<JointType, Joint> joint2 = Utility.GetValidJoints(bodies[j].Joints);
                if (validFlags[0] && validFlags[j] == false)
                {
                    continue;
                }
                ICoordConversion3D crtc = new CoordRotTransConversion();
                foreach (JointType jointType in Enum.GetValues(typeof(JointType)))
                {
                    if (!joint1.ContainsKey(jointType))
                        continue;
                    if (!joint2.ContainsKey(jointType))
                        continue;
                    CvPoint3D64f from = joint2[jointType].Position.ToCvPoint3D();
                    CvPoint3D64f target = CvEx.ConvertPoint3D(joint1[jointType].Position.ToCvPoint3D(), convList[0]);
                    // IsOriginlJointValid相当の処理を入れるかどうか
                    crtc.PutPoint(from, target, 1);
                }
                convList[j] = crtc.Solve();
            }
            return convList;
        }