Initial push of JointICP