@@ -89,7 +89,6 @@ bool SL4::equals(const SL4& sl4, double tol) const {
8989}
9090/* ************************************************************************* */
9191SL4 SL4::ChartAtOrigin::Retract (const Vector15& v, ChartJacobian H) {
92- assert (v.size () == 15 );
9392 SL4 retracted (I_4x4 + Hat (v));
9493 if (H) throw std::runtime_error (" SL4::Retract: Jacobian not implemented." );
9594 return retracted;
@@ -104,7 +103,11 @@ Vector15 SL4::ChartAtOrigin::Local(const SL4& sl4, ChartJacobian H) {
104103
105104/* ************************************************************************* */
106105SL4 SL4::Expmap (const Vector& xi, SL4Jacobian H) {
107- assert (xi.size () == 15 );
106+ if (xi.size () != 15 ) {
107+ throw std::runtime_error (
108+ " SL4::Expmap: xi must be a vector of size 15. Got size " +
109+ std::to_string (xi.size ()));
110+ }
108111 const auto & A = Hat (xi);
109112
110113 if (H) throw std::runtime_error (" SL4::Expmap: Jacobian not implemented." );
@@ -139,7 +142,11 @@ Matrix15x15 SL4::AdjointMap() const {
139142
140143/* ************************************************************************* */
141144Matrix44 SL4::Hat (const Vector& xi) {
142- assert (xi.size () == 15 );
145+ if (xi.size () != 15 ) {
146+ throw std::runtime_error (
147+ " SL4::Hat: xi must be a vector of size 15. Got size " +
148+ std::to_string (xi.size ()));
149+ }
143150 Matrix44 A;
144151 const double d11 = xi (12 );
145152 const double d22 = -xi (12 ) + xi (13 );
0 commit comments