* Validate and convert parsed data to [T, 4, 4] format * @param {Array} data * @param {string} format * @returns {Object} - { valid: boolean, data: Array, error: string|null }
(data, format)
| 299 | * @returns {Object} - { valid: boolean, data: Array, error: string|null } |
| 300 | */ |
| 301 | static validateAndConvert(data, format) { |
| 302 | if (!Array.isArray(data)) { |
| 303 | return { valid: false, error: 'Input must be an array' }; |
| 304 | } |
| 305 | |
| 306 | if (data.length === 0) { |
| 307 | return { valid: false, error: 'Array is empty' }; |
| 308 | } |
| 309 | |
| 310 | const convertedData = []; |
| 311 | |
| 312 | // Check each pose based on format |
| 313 | for (let i = 0; i < data.length; i++) { |
| 314 | const pose = data[i]; |
| 315 | |
| 316 | if (!Array.isArray(pose)) { |
| 317 | return { valid: false, error: `Pose at index ${i} is not an array` }; |
| 318 | } |
| 319 | |
| 320 | if (format === 'matrix') { |
| 321 | // Expect [4, 4] |
| 322 | if (pose.length !== 4) { |
| 323 | return { valid: false, error: `Pose at index ${i} should have 4 rows, got ${pose.length}` }; |
| 324 | } |
| 325 | |
| 326 | for (let j = 0; j < 4; j++) { |
| 327 | if (!Array.isArray(pose[j])) { |
| 328 | return { valid: false, error: `Row ${j} of pose ${i} is not an array` }; |
| 329 | } |
| 330 | if (pose[j].length !== 4) { |
| 331 | return { valid: false, error: `Row ${j} of pose ${i} should have 4 columns, got ${pose[j].length}` }; |
| 332 | } |
| 333 | for (let k = 0; k < 4; k++) { |
| 334 | if (typeof pose[j][k] !== 'number' || isNaN(pose[j][k])) { |
| 335 | return { valid: false, error: `Element [${j}][${k}] of pose ${i} is not a valid number` }; |
| 336 | } |
| 337 | } |
| 338 | } |
| 339 | convertedData.push(pose); |
| 340 | |
| 341 | } else if (format === 'xyz_quat_wxyz' || format === 'xyz_quat_xyzw' || format === 'quat_wxyz_xyz') { |
| 342 | // Expect [7] (x, y, z, q1, q2, q3, q4) |
| 343 | if (pose.length !== 7) { |
| 344 | return { valid: false, error: `Pose at index ${i} should have 7 elements, got ${pose.length}` }; |
| 345 | } |
| 346 | |
| 347 | for (let k = 0; k < 7; k++) { |
| 348 | if (typeof pose[k] !== 'number' || isNaN(pose[k])) { |
| 349 | return { valid: false, error: `Element ${k} of pose ${i} is not a valid number` }; |
| 350 | } |
| 351 | } |
| 352 | |
| 353 | // Convert to 4x4 matrix |
| 354 | let x, y, z, qw, qx, qy, qz; |
| 355 | |
| 356 | if (format === 'quat_wxyz_xyz') { |
| 357 | // qw, qx, qy, qz, x, y, z |
| 358 | qw = pose[0]; |