internal void RecoverCalibrationFromSensor()

in Sources/Kinect/Microsoft.Psi.Kinect.Windows/KinectInternalCalibration.cs [32:169]
107 lines of code
16 McCabe index (conditional complexity)

        internal void RecoverCalibrationFromSensor(Microsoft.Kinect.KinectSensor kinectSensor)
        {
            var stopWatch = new System.Diagnostics.Stopwatch();
            stopWatch.Start();

            var objectPoints1 = new List<Vector<double>>();
            var colorPoints1 = new List<System.Drawing.PointF>();
            var depthPoints1 = new List<System.Drawing.PointF>();

            int n = 0;
            for (float x = -2f; x < 2f; x += 0.2f)
                for (float y = -2f; y < 2f; y += 0.2f)
                    for (float z = 0.4f; z < 4.5f; z += 0.4f)
                    {
                        var kinectCameraPoint = new CameraSpacePoint();
                        kinectCameraPoint.X = x;
                        kinectCameraPoint.Y = y;
                        kinectCameraPoint.Z = z;

                        // use SDK's projection
                        // adjust Y to make RH coordinate system that is a projection of Kinect 3D points
                        var kinectColorPoint = kinectSensor.CoordinateMapper.MapCameraPointToColorSpace(kinectCameraPoint);
                        kinectColorPoint.Y = colorImageHeight - kinectColorPoint.Y;
                        var kinectDepthPoint = kinectSensor.CoordinateMapper.MapCameraPointToDepthSpace(kinectCameraPoint);
                        kinectDepthPoint.Y = depthImageHeight - kinectDepthPoint.Y;

                        if ((kinectColorPoint.X >= 0) && (kinectColorPoint.X < colorImageWidth) &&
                            (kinectColorPoint.Y >= 0) && (kinectColorPoint.Y < colorImageHeight) &&
                            (kinectDepthPoint.X >= 0) && (kinectDepthPoint.X < depthImageWidth) &&
                            (kinectDepthPoint.Y >= 0) && (kinectDepthPoint.Y < depthImageHeight))
                        {
                            n++;
                            var objectPoint = Vector<double>.Build.Dense(3);
                            objectPoint[0] = kinectCameraPoint.X;
                            objectPoint[1] = kinectCameraPoint.Y;
                            objectPoint[2] = kinectCameraPoint.Z;
                            objectPoints1.Add(objectPoint);

                            var colorPoint = new System.Drawing.PointF();
                            colorPoint.X = kinectColorPoint.X;
                            colorPoint.Y = kinectColorPoint.Y;
                            colorPoints1.Add(colorPoint);


                            //Console.WriteLine(objectPoint[0] + "\t" + objectPoint[1] + "\t" + colorPoint.X + "\t" + colorPoint.Y);

                            var depthPoint = new System.Drawing.PointF();
                            depthPoint.X = kinectDepthPoint.X;
                            depthPoint.Y = kinectDepthPoint.Y;
                            depthPoints1.Add(depthPoint);
                        }
                    }

            this.colorCameraMatrix[0, 0] = 1000; //fx
            this.colorCameraMatrix[1, 1] = 1000; //fy
            this.colorCameraMatrix[0, 2] = colorImageWidth / 2; //cx
            this.colorCameraMatrix[1, 2] = colorImageHeight / 2; //cy
            this.colorCameraMatrix[2, 2] = 1;

            var rotation = Vector<double>.Build.Dense(3);
            var translation = Vector<double>.Build.Dense(3);
            var colorError = CalibrateColorCamera(objectPoints1, colorPoints1, colorCameraMatrix, colorLensDistortion, rotation, translation, this.silent);
            var rotationMatrix = AxisAngleToMatrix(rotation);

            this.depthToColorTransform = Matrix<double>.Build.DenseIdentity(4, 4);
            for (int i = 0; i < 3; i++)
            {
                this.depthToColorTransform[i, 3] = translation[i];
                for (int j = 0; j < 3; j++)
                    this.depthToColorTransform[i, j] = rotationMatrix[i, j];
            }


            this.depthCameraMatrix[0, 0] = 360; //fx
            this.depthCameraMatrix[1, 1] = 360; //fy
            this.depthCameraMatrix[0, 2] = depthImageWidth / 2.0; //cx
            this.depthCameraMatrix[1, 2] = depthImageHeight / 2.0; //cy
            this.depthCameraMatrix[2, 2] = 1;

            var depthError = CalibrateDepthCamera(objectPoints1, depthPoints1, depthCameraMatrix, depthLensDistortion, silent);

            // check projections
            double depthProjectionError = 0;
            double colorProjectionError = 0;
            var testObjectPoint4 = Vector<double>.Build.Dense(4);
            for (int i = 0; i < n; i++)
            {
                var testObjectPoint = objectPoints1[i];
                var testDepthPoint = depthPoints1[i];
                var testColorPoint = colorPoints1[i];

                // "camera space" == depth camera space
                // depth camera projection
                double depthU, depthV;
                Project(depthCameraMatrix, depthLensDistortion, testObjectPoint[0], testObjectPoint[1], testObjectPoint[2], out depthU, out depthV);

                double dx = testDepthPoint.X - depthU;
                double dy = testDepthPoint.Y - depthV;
                depthProjectionError += (dx * dx) + (dy * dy);

                // color camera projection
                testObjectPoint4[0] = testObjectPoint[0];
                testObjectPoint4[1] = testObjectPoint[1];
                testObjectPoint4[2] = testObjectPoint[2];
                testObjectPoint4[3] = 1;

                var color = depthToColorTransform * testObjectPoint4;
                color *= (1.0 / color[3]); // not necessary for this transform

                double colorU, colorV;
                Project(colorCameraMatrix, colorLensDistortion, color[0], color[1], color[2], out colorU, out colorV);

                dx = testColorPoint.X - colorU;
                dy = testColorPoint.Y - colorV;
                colorProjectionError += (dx * dx) + (dy * dy);
            }
            depthProjectionError /= n;
            colorProjectionError /= n;


            stopWatch.Stop();
            if (!this.silent)
            {
                Console.WriteLine("FakeCalibration :");
                Console.WriteLine("n = " + n);
                Console.WriteLine("color error = " + colorError);
                Console.WriteLine("depth error = " + depthError);
                Console.WriteLine("depth reprojection error = " + depthProjectionError);
                Console.WriteLine("color reprojection error = " + colorProjectionError);
                Console.WriteLine("depth camera matrix = \n" + depthCameraMatrix);
                Console.WriteLine("depth lens distortion = \n" + depthLensDistortion);
                Console.WriteLine("color camera matrix = \n" + colorCameraMatrix);
                Console.WriteLine("color lens distortion = \n" + colorLensDistortion);

                Console.WriteLine(stopWatch.ElapsedMilliseconds + " ms");
                Console.WriteLine("________________________________________________________");
            }
        }