Feature/335 initialize icp

src/architecture/msgPayloadDefCpp/SICPMsgPayload.h

@@ -32,7 +32,9 @@ typedef struct
 SICPMsgPayload
 //@endcond
 {
+    bool valid; //!< --[-] Message was purposefully populated
     uint64_t numberOfIteration; //!< --[-] Number of iterations used for SICP convergence
+    uint64_t timeTag; //!< --[-] Time at which these iterations were computed
     double scaleFactor[MAX_ITERATIONS]; //!< -- [-] Array of scale factors
     double rotationMatrix[POINT_DIM*POINT_DIM*MAX_ITERATIONS]; //!< -- [-]  Array of rotation matrices
     double translation[POINT_DIM*MAX_ITERATIONS]; //!< -- [-] Array of translation vectors

src/fswAlgorithms/pointCloudProcessing/SICP/_UnitTest/testFunctions.py

@@ -183,6 +183,7 @@ def runModule(data, reference, numberPoints, iters = 100):
     # Create the input messages.
     inputPointCloud = messaging.PointCloudMsgPayload()
     referencePointCloud = messaging.PointCloudMsgPayload()
+    icpInitialCondition = messaging.SICPMsgPayload()
 
     inputPointCloud.points = data.flatten().tolist()
     inputPointCloud.numberOfPoints = numberPoints
@@ -191,6 +192,10 @@ def runModule(data, reference, numberPoints, iters = 100):
     inputPointCloudMsg = messaging.PointCloudMsg().write(inputPointCloud)
     sicp.measuredPointCloud.subscribeTo(inputPointCloudMsg)
 
+    icpInitialCondition.valid = False
+    initialConditionMsg = messaging.SICPMsg().write(icpInitialCondition)
+    sicp.initialCondition.subscribeTo(initialConditionMsg)
+
     referencePointCloud.points = reference.flatten().tolist()
     referencePointCloud.numberOfPoints = numberPoints
     referencePointCloud.timeTag = 0

src/fswAlgorithms/pointCloudProcessing/SICP/scalingIterativeClosestPoint.cpp

@@ -36,6 +36,9 @@ void ScalingIterativeClosestPoint::Reset(uint64_t CurrentSimNanos)
     if (!this->referencePointCloud.isLinked()) {
         bskLogger.bskLog(BSK_ERROR, "Measured Point Cloud wasn't connected.");
     }
+    if (!this->initialCondition.isLinked()) {
+        bskLogger.bskLog(BSK_ERROR, "Initial condition message wasn't connected.");
+    }
 }
 
 /*! Compute the closest reference point to each measured point
@@ -196,6 +199,27 @@ void ScalingIterativeClosestPoint::UpdateState(uint64_t CurrentSimNanos)
 
     this->measuredCloudBuffer = this->measuredPointCloud();
     this->referenceCloudBuffer = this->referencePointCloud();
+    bool initicalConditionValidity = false;
+    if (this->initialCondition.isLinked()) {
+        this->initialConditionBuffer = this->initialCondition();
+        initicalConditionValidity = this->initialConditionBuffer.valid;
+    }
+
+    //! - If initial condition message exists populate the initial conditions, otherwise use defaults
+    if (initicalConditionValidity) {
+        this->R_init = cArray2EigenMatrixXd(this->initialConditionBuffer.rotationMatrix,
+                                            POINT_DIM,
+                                            POINT_DIM);
+        this->t_init = Eigen::Map<Eigen::VectorXd>(this->initialConditionBuffer.translation,
+                                                   POINT_DIM,
+                                                   1);
+        this->s_init = this->initialConditionBuffer.scaleFactor[0];
+    }
+    else {
+        this->R_init = Eigen::MatrixXd::Identity(POINT_DIM, POINT_DIM);
+        this->t_init = Eigen::VectorXd::Zero(POINT_DIM);
+        this->s_init = 1;
+    }
 
     //! - Check for data validity
     if (!this->measuredCloudBuffer.valid) {
@@ -230,7 +254,7 @@ void ScalingIterativeClosestPoint::UpdateState(uint64_t CurrentSimNanos)
             //! - Center point clouds about average point
             this->centerCloud(measuredPoints);
 
-            for (int iterRS = 0; iterRS < this->maxInteralIterations; iterRS++) {
+            for (int iterRS = 0; iterRS < this->maxInternalIterations; iterRS++) {
                 //! - Eq 14-17 to find R
                 R_k = this->computeRk(s_kmin1, R_kmin1);
 
@@ -266,6 +290,8 @@ void ScalingIterativeClosestPoint::UpdateState(uint64_t CurrentSimNanos)
 
         //! - Save the algorithm output data if the measurement was valid
         this->outputCloudBuffer.valid = true;
+        this->sicpBuffer.valid = true;
+        this->sicpBuffer.timeTag = CurrentSimNanos;
         Eigen::MatrixXd newPoints = Eigen::MatrixXd::Zero(measuredPoints.rows(), measuredPoints.cols());
         for (int i = 0; i < this->Np; i++) {
             newPoints.col(i) = s_k * (R_k * measuredPoints.col(i)) + t_k;

src/fswAlgorithms/pointCloudProcessing/SICP/scalingIterativeClosestPoint.h

@@ -39,11 +39,12 @@ class ScalingIterativeClosestPoint: public SysModel {
     ScalingIterativeClosestPoint();
     ~ScalingIterativeClosestPoint();
 
-    void UpdateState(uint64_t CurrentSimNanos);
-    void Reset(uint64_t CurrentSimNanos);
+    void UpdateState(uint64_t CurrentSimNanos) override;
+    void Reset(uint64_t CurrentSimNanos) override;
 
     Message<PointCloudMsgPayload> outputPointCloud;  //!< The output fitted point cloud
     Message<SICPMsgPayload> outputSICPData;  //!< The output algorithm data
+    ReadFunctor<SICPMsgPayload> initialCondition;          //!< The input measured data
     ReadFunctor<PointCloudMsgPayload> measuredPointCloud;          //!< The input measured data
     ReadFunctor<PointCloudMsgPayload> referencePointCloud;          //!< The input reference data
     BSKLogger bskLogger;                //!< -- BSK Logging
@@ -70,14 +71,15 @@ class ScalingIterativeClosestPoint: public SysModel {
     PointCloudMsgPayload outputCloudBuffer;
     PointCloudMsgPayload measuredCloudBuffer;
     PointCloudMsgPayload referenceCloudBuffer;
+    SICPMsgPayload initialConditionBuffer;
     SICPMsgPayload sicpBuffer;
 
     Eigen::MatrixXd correspondingPoints;
     Eigen::MatrixXd q;
     Eigen::MatrixXd n;
 
     int Np = 0; //!< Number of detected points
-    int maxInteralIterations = 10; //!< Maximum iterations in the inner loop for scale factor and rotation
+    int maxInternalIterations = 10; //!< Maximum iterations in the inner loop for scale factor and rotation
 
 };
 

src/fswAlgorithms/pointCloudProcessing/SICP/sicpDefinitions.h

@@ -19,4 +19,4 @@
 
 #define MAX_POINTS 5000
 #define POINT_DIM 3
-#define MAX_ITERATIONS 100
+#define MAX_ITERATIONS 250

src/fswAlgorithms/pointCloudProcessing/initializeICP/_UnitTest/test_initICP.py

@@ -0,0 +1,187 @@
+# ISC License
+#
+# Copyright (c) 2023, Laboratory for Atmospheric Space Physics, University of Colorado Boulder
+#
+# Permission to use, copy, modify, and/or distribute this software for any
+# purpose with or without fee is hereby granted, provided that the above
+# copyright notice and this permission notice appear in all copies.
+#
+# THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+# WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+# MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
+# ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+# WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
+# ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
+# OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+
+import inspect
+import numpy as np
+import os
+
+filename = inspect.getframeinfo(inspect.currentframe()).filename
+path = os.path.dirname(os.path.abspath(filename))
+bskName = 'Basilisk'
+splitPath = path.split(bskName)
+
+from Basilisk.utilities import RigidBodyKinematics as rbk
+from Basilisk.utilities import SimulationBaseClass, macros
+
+from Basilisk.fswAlgorithms import initializeICP
+from Basilisk.architecture import messaging
+
+
+def test_all_valid():
+    """
+    Unit test with valid ICP message and valid point cloud
+    """
+    icp_init_sim(validICP = True, validCloud = True, normalize = True)
+
+def test_all_valid_no_normalization():
+    """
+    Unit test with valid ICP message and valid point cloud without normalization
+    """
+    icp_init_sim(validICP = True, validCloud = True, normalize = False)
+
+def test_valid_cloud():
+    """
+    Unit test with valid ICP message and valid point cloud
+    """
+    icp_init_sim(validICP = False, validCloud = True, normalize = True)
+
+def test_valid_ICP():
+    """
+    Unit test with valid ICP message and valid point cloud
+    """
+    icp_init_sim(validICP = True, validCloud = False, normalize = True)
+
+
+def icp_init_sim(validICP = True, validCloud = True, normalize = True):
+    unit_task_name = "unitTask"
+    unit_process_name = "TestProcess"
+
+    unit_test_sim = SimulationBaseClass.SimBaseClass()
+    process_rate = macros.sec2nano(0.5)
+    test_process = unit_test_sim.CreateNewProcess(unit_process_name)
+    test_process.addTask(unit_test_sim.CreateNewTask(unit_task_name, process_rate))
+
+    # setup module to be tested
+    module = initializeICP.InitializeICP()
+    # module.ModelTag = 'initializationICP'
+    module.normalizeMeasuredCloud = normalize
+    unit_test_sim.AddModelToTask(unit_task_name, module)
+
+    ephemeris_input_msg_buffer = messaging.EphemerisMsgPayload()
+    ephemeris_input_msg_buffer.r_BdyZero_N = [100, 1, 0]  # use v_C1_N instead of v_BN_N for accurate unit test
+    ephemeris_input_msg_buffer.sigma_BN = [0.1, 0., 1]  # use v_C1_N instead of v_BN_N for accurate unit test
+    ephemeris_input_msg_buffer.timeTag = 1 * 1E9
+    ephemeris_input_msg = messaging.EphemerisMsg().write(ephemeris_input_msg_buffer)
+    module.ephemerisInMsg.subscribeTo(ephemeris_input_msg)
+
+    camera_input_msg_buffer = messaging.CameraConfigMsgPayload()
+    camera_input_msg_buffer.cameraID = 1
+    camera_input_msg_buffer.sigma_CB = [0.1, 0.2, 0.3]
+    camera_input_msg = messaging.CameraConfigMsg().write(camera_input_msg_buffer)
+    module.cameraConfigInMsg.subscribeTo(camera_input_msg)
+
+    input_points = np.array([[1, 1, 1], [2, 2, 2], [5, 1, 3], [0, 2, 0], [0, 0, 1], [0, 0, -1]])
+    pointcloud_input_msg_buffer = messaging.PointCloudMsgPayload()
+    pointcloud_input_msg_buffer.points = input_points.flatten().tolist()
+    pointcloud_input_msg_buffer.numberOfPoints = len(input_points)
+    pointcloud_input_msg_buffer.timeTag = 1
+    pointcloud_input_msg_buffer.valid = validCloud
+    pointcloud_input_msg = messaging.PointCloudMsg().write(pointcloud_input_msg_buffer)
+    module.inputMeasuredPointCloud.subscribeTo(pointcloud_input_msg)
+
+    icp_input_msg_buffer = messaging.SICPMsgPayload()
+    icp_input_msg_buffer.rotationMatrix = [1, 0, 0, 0, 1, 0, 0, 0, 1]
+    icp_input_msg_buffer.translation = [1, 0, 0]
+    icp_input_msg_buffer.scaleFactor = [1.05]
+    icp_input_msg_buffer.numberOfIteration = 1
+    icp_input_msg_buffer.valid = validICP
+    icp_input_msg_buffer.timeTag = 500
+    icp_input_msg = messaging.SICPMsg().write(icp_input_msg_buffer)
+    module.inputSICPData.subscribeTo(icp_input_msg)
+
+    icp_msg_log = module.initializeSICPMsg.recorder()
+    unit_test_sim.AddModelToTask(unit_task_name, icp_msg_log)
+    cloud_msg_log = module.measuredPointCloud.recorder()
+    unit_test_sim.AddModelToTask(unit_task_name, cloud_msg_log)
+
+    unit_test_sim.InitializeSimulation()
+    unit_test_sim.ConfigureStopTime(process_rate)
+    unit_test_sim.ExecuteSimulation()
+
+    output_iterations = icp_msg_log.numberOfIteration[-1] + 1
+    output_Ss = icp_msg_log.scaleFactor[-1][:output_iterations]
+    output_Rs = icp_msg_log.rotationMatrix[-1][:9*output_iterations]
+    output_Ts = icp_msg_log.translation[-1][:3*output_iterations]
+
+    output_cloud_valid = cloud_msg_log.valid[-1]
+    output_cloud_timetag = cloud_msg_log.timeTag[-1]
+    output_cloud_numberpoints = cloud_msg_log.numberOfPoints[-1]
+    output_pointcloud = cloud_msg_log.points[-1][:3*output_cloud_numberpoints]
+
+    point_cloud_module = np.array(output_pointcloud).reshape([output_cloud_numberpoints, 3])
+
+    accuracy = 1E-10
+    if module.normalizeMeasuredCloud:
+        avg = np.mean(np.linalg.norm(input_points, axis=1))
+    else:
+        avg = 1
+    if validCloud and validICP:
+        np.testing.assert_allclose(point_cloud_module,
+                                   input_points/avg,
+                                   rtol=0,
+                                   atol=accuracy,
+                                   err_msg=('Normalized point cloud vs expected'),
+                                   verbose=True)
+        np.testing.assert_allclose(output_Ss,
+                                   icp_input_msg_buffer.scaleFactor[0:1],
+                                   rtol=0,
+                                   atol=accuracy,
+                                   err_msg=('Scale: ICP parameters vs expected'),
+                                   verbose=True)
+        np.testing.assert_allclose(output_Ts,
+                                   icp_input_msg_buffer.translation[0:3],
+                                   rtol=0,
+                                   atol=accuracy,
+                                   err_msg=('Translation: ICP parameters vs expected'),
+                                   verbose=True)
+        np.testing.assert_allclose(output_Rs,
+                                   icp_input_msg_buffer.rotationMatrix[0:9],
+                                   rtol=0,
+                                   atol=accuracy,
+                                   err_msg=('Rotation: ICP parameters vs expected'),
+                                   verbose=True)
+    if not validCloud:
+        np.testing.assert_allclose(point_cloud_module,
+                                   np.zeros(np.shape(point_cloud_module)),
+                                   rtol=0,
+                                   atol=accuracy,
+                                   err_msg=('Normalized point cloud vs expected'),
+                                   verbose=True)
+    if not validICP and validCloud:
+        np.testing.assert_allclose(output_Ss,
+                                   [1],
+                                   rtol=0,
+                                   atol=accuracy,
+                                   err_msg=('Invalid ICP: ICP parameters vs expected'),
+                                   verbose=True)
+        np.testing.assert_allclose(output_Ts,
+                                   np.array(ephemeris_input_msg_buffer.r_BdyZero_N),
+                                   rtol=0,
+                                   atol=accuracy,
+                                   err_msg=('Invalid ICP: ICP parameters vs expected'),
+                                   verbose=True)
+        BN = rbk.MRP2C(ephemeris_input_msg_buffer.sigma_BN)
+        CB = rbk.MRP2C(camera_input_msg_buffer.sigma_CB)
+        np.testing.assert_allclose(np.array(output_Rs).reshape([3, 3]),
+                                   np.dot(CB, BN),
+                                   rtol=0,
+                                   atol=accuracy,
+                                   err_msg=('Invalid ICP: ICP parameters vs expected'),
+                                   verbose=True)
+
+
+if __name__ == "__main__":
+    icp_init_sim()