TEST(UncertainHomogeneousPointTestSuite, testU3)
{
  using namespace sm::kinematics;
  Eigen::Vector3d p;
  p.setRandom();
  Eigen::Matrix3d U;
  U = sm::eigen::randomCovariance<3>();

  UncertainHomogeneousPoint uhp(p,U);
  
  sm::eigen::assertNear(U, uhp.U3(), 1e-10, SM_SOURCE_FILE_POS, "Checking that I can recover the Euclideam point uncertainty");
}
TEST(UncertainHomogeneousPointTestSuite, testU3normalized)
{
  using namespace sm::kinematics;
  Eigen::Vector3d p;
  p.setRandom();
  Eigen::Matrix3d U;
  U = sm::eigen::randomCovariance<3>();

  UncertainHomogeneousPoint uhp(p,U);

  uhp.normalize();
  
  ASSERT_NEAR(uhp.toHomogeneous().norm(),1.0,1e-14);
  sm::eigen::assertNear(p, uhp.toEuclidean(),1e-10, SM_SOURCE_FILE_POS, "Checking that I can recover the Euclidean point after normalization");
  sm::eigen::assertNear(U, uhp.U3(), 1e-10, SM_SOURCE_FILE_POS, "Checking that I can recover the Euclideam point uncertainty after normalization.");
}