From 9fbf770335963f10598bb8bfd4824dbeba5781f0 Mon Sep 17 00:00:00 2001 From: raphaelchang Date: Tue, 17 Sep 2019 22:47:15 -0400 Subject: [PATCH] Add five point algorithm for checking essential matrix outliers --- CMakeLists.txt | 3 +- launch/odometry_eval.launch | 4 +- launch/reconstruction_eval.launch | 4 +- launch/tracking_eval.launch | 4 +- src/feature/tracker.cc | 33 +++--- src/feature/tracker.h | 1 + src/module/tracking_module.cc | 25 +++- src/module/tracking_module.h | 6 +- src/odometry/five_point.cc | 186 ++++++++++++++++++++++++++++++ src/odometry/five_point.h | 35 ++++++ src/odometry/pnp.cc | 15 ++- src/odometry/pnp.h | 1 + src/ros/tracking_eval.cc | 9 +- src/util/math_util.h | 47 ++++++++ 14 files changed, 346 insertions(+), 27 deletions(-) create mode 100644 src/odometry/five_point.cc create mode 100644 src/odometry/five_point.h diff --git a/CMakeLists.txt b/CMakeLists.txt index 6424128..d972d03 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -6,7 +6,7 @@ set(CMAKE_VERBOSE_MAKEFILE FALSE) add_definitions("-std=c++17") if (NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE "Release" CACHE STRING "Choose the type of build, options are: Debug Release RelWithDebInfo MinSizeRel." FORCE) + set(CMAKE_BUILD_TYPE "RelWithDebInfo" CACHE STRING "Choose the type of build, options are: Debug Release RelWithDebInfo MinSizeRel." FORCE) endif (NOT CMAKE_BUILD_TYPE) # find catkin dependencies @@ -68,6 +68,7 @@ add_library(omni_slam_eval_lib src/feature/matcher.cc src/reconstruction/triangulator.cc src/odometry/pnp.cc + src/odometry/five_point.cc src/optimization/bundle_adjuster.cc src/util/hdf_file.cc ) diff --git a/launch/odometry_eval.launch b/launch/odometry_eval.launch index dabcd58..072505f 100644 --- a/launch/odometry_eval.launch +++ b/launch/odometry_eval.launch @@ -23,7 +23,9 @@ detector_parameters: {maxCorners: 2000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} tracker_window_size: 128 tracker_num_scales: 4 - tracker_delta_pixel_error_threshold: 3.0 + tracker_checker_epipolar_threshold: 0.008 + tracker_checker_iterations: 1000 + tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 20.0 min_features_per_region: 100 pnp_inlier_threshold: 3.0 diff --git a/launch/reconstruction_eval.launch b/launch/reconstruction_eval.launch index ef58dca..662a3d9 100644 --- a/launch/reconstruction_eval.launch +++ b/launch/reconstruction_eval.launch @@ -20,7 +20,9 @@ detector_parameters: {maxCorners: 5000, qualityLevel: 0.001, minDistance: 5, blockSize: 5} tracker_window_size: 128 tracker_num_scales: 4 - tracker_delta_pixel_error_threshold: 3.0 + tracker_checker_epipolar_threshold: 0.008 + tracker_checker_iterations: 1000 + tracker_delta_pixel_error_threshold: 0.0 tracker_error_threshold: 20.0 min_features_per_region: 10 max_reprojection_error: 5.0 diff --git a/launch/tracking_eval.launch b/launch/tracking_eval.launch index 452b00b..f75348a 100644 --- a/launch/tracking_eval.launch +++ b/launch/tracking_eval.launch @@ -2,7 +2,7 @@ - + @@ -19,6 +19,8 @@ detector_parameters: {maxCorners: 100, qualityLevel: 0.01, minDistance: 5, blockSize: 5} tracker_window_size: 128 tracker_num_scales: 4 + tracker_checker_epipolar_threshold: 0.008 + tracker_checker_iterations: 1000 tracker_delta_pixel_error_threshold: 3.0 tracker_error_threshold: 20.0 min_features_per_region: 10 diff --git a/src/feature/tracker.cc b/src/feature/tracker.cc index e592c6c..454712d 100644 --- a/src/feature/tracker.cc +++ b/src/feature/tracker.cc @@ -59,26 +59,29 @@ int Tracker::Track(std::vector &landmarks, data::Frame &cur_fram for (int i = 0; i < results.size(); i++) { data::Landmark &landmark = landmarks[orig_inx[i]]; - if (landmark.HasGroundTruth() && cur_frame.HasPose()) + if (deltaPixErrThresh_ > 0) { - Vector2d pixel_gnd; - Vector2d pixel_gnd_prev; - if (!cur_frame.GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(cur_frame.GetInversePose(), landmark.GetGroundTruth())), pixel_gnd) || !prevFrame_->GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(prevFrame_->GetInversePose(), landmark.GetGroundTruth())), pixel_gnd_prev)) + if (landmark.HasGroundTruth() && cur_frame.HasPose()) { - continue; - } - else - { - Vector2d pixel_cur; - pixel_cur << results[i].x, results[i].y; - Vector2d pixel_prev; - pixel_prev << points_to_track[i].x, points_to_track[i].y; - double cur_error = (pixel_cur - pixel_gnd).norm(); - double prev_error = (pixel_prev - pixel_gnd_prev).norm(); - if (cur_error - prev_error > deltaPixErrThresh_) + Vector2d pixel_gnd; + Vector2d pixel_gnd_prev; + if (!cur_frame.GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(cur_frame.GetInversePose(), landmark.GetGroundTruth())), pixel_gnd) || !prevFrame_->GetCameraModel().ProjectToImage(util::TFUtil::WorldFrameToCameraFrame(util::TFUtil::TransformPoint(prevFrame_->GetInversePose(), landmark.GetGroundTruth())), pixel_gnd_prev)) { continue; } + else + { + Vector2d pixel_cur; + pixel_cur << results[i].x, results[i].y; + Vector2d pixel_prev; + pixel_prev << points_to_track[i].x, points_to_track[i].y; + double cur_error = (pixel_cur - pixel_gnd).norm(); + double prev_error = (pixel_prev - pixel_gnd_prev).norm(); + if (cur_error - prev_error > deltaPixErrThresh_) + { + continue; + } + } } } if (status[i] == 1 && err[i] <= errThresh_) diff --git a/src/feature/tracker.h b/src/feature/tracker.h index 1ec1120..f9ad316 100644 --- a/src/feature/tracker.h +++ b/src/feature/tracker.h @@ -4,6 +4,7 @@ #include #include "data/frame.h" #include "data/landmark.h" +#include "odometry/five_point.h" #include namespace omni_slam diff --git a/src/module/tracking_module.cc b/src/module/tracking_module.cc index e0eb156..d7ae4cc 100644 --- a/src/module/tracking_module.cc +++ b/src/module/tracking_module.cc @@ -11,15 +11,16 @@ namespace omni_slam namespace module { -TrackingModule::TrackingModule(std::unique_ptr &detector, std::unique_ptr &tracker, int minFeaturesRegion) +TrackingModule::TrackingModule(std::unique_ptr &detector, std::unique_ptr &tracker, std::unique_ptr &checker, int minFeaturesRegion) : detector_(std::move(detector)), tracker_(std::move(tracker)), + fivePointChecker_(std::move(checker)), minFeaturesRegion_(minFeaturesRegion) { } -TrackingModule::TrackingModule(std::unique_ptr &&detector, std::unique_ptr &&tracker, int minFeaturesRegion) - : TrackingModule(detector, tracker, minFeaturesRegion) +TrackingModule::TrackingModule(std::unique_ptr &&detector, std::unique_ptr &&tracker, std::unique_ptr &&checker, int minFeaturesRegion) + : TrackingModule(detector, tracker, checker, minFeaturesRegion) { } @@ -47,7 +48,23 @@ void TrackingModule::Update(std::unique_ptr &frame) } vector trackErrors; - tracker_->Track(landmarks_, *frames_.back(), trackErrors); + std::vector landmarksTemp(landmarks_); + tracker_->Track(landmarksTemp, *frames_.back(), trackErrors); + + if (fivePointChecker_) + { + Matrix3d E; + std::vector inlierIndices; + fivePointChecker_->Compute(landmarksTemp, **next(frames_.rbegin()), *frames_.back(), E, inlierIndices); + for (int inx : inlierIndices) + { + landmarks_[inx].AddObservation(*landmarksTemp[inx].GetObservationByFrameID(frames_.back()->GetID())); + } + } + else + { + landmarks_ = std::move(landmarksTemp); + } int i = 0; int numGood = 0; diff --git a/src/module/tracking_module.h b/src/module/tracking_module.h index be70706..8fe464d 100644 --- a/src/module/tracking_module.h +++ b/src/module/tracking_module.h @@ -7,6 +7,7 @@ #include "feature/tracker.h" #include "feature/detector.h" +#include "odometry/five_point.h" #include "data/frame.h" #include "data/landmark.h" @@ -27,8 +28,8 @@ class TrackingModule std::vector failureRadDists; }; - TrackingModule(std::unique_ptr &detector, std::unique_ptr &tracker, int minFeaturesRegion = 5); - TrackingModule(std::unique_ptr &&detector, std::unique_ptr &&tracker, int minFeaturesRegion = 5); + TrackingModule(std::unique_ptr &detector, std::unique_ptr &tracker, std::unique_ptr &checker, int minFeaturesRegion = 5); + TrackingModule(std::unique_ptr &&detector, std::unique_ptr &&tracker, std::unique_ptr &&checker, int minFeaturesRegion = 5); void Update(std::unique_ptr &frame); void Redetect(); @@ -57,6 +58,7 @@ class TrackingModule std::shared_ptr detector_; std::shared_ptr tracker_; + std::shared_ptr fivePointChecker_; std::vector> frames_; std::vector landmarks_; diff --git a/src/odometry/five_point.cc b/src/odometry/five_point.cc new file mode 100644 index 0000000..2a2f21a --- /dev/null +++ b/src/odometry/five_point.cc @@ -0,0 +1,186 @@ +#include "five_point.h" + +#include "util/math_util.h" +#include +#include + +namespace omni_slam +{ +namespace odometry +{ + +FivePoint::FivePoint(int ransac_iterations, double epipolar_threshold) + : ransacIterations_(ransac_iterations), + epipolarThreshold_(epipolar_threshold) +{ +} + +int FivePoint::Compute(const std::vector &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, std::vector &inlier_indices) const +{ + std::vector x1; + std::vector x2; + std::map indexToLandmarkIndex; + int i = 0; + for (const data::Landmark &landmark : landmarks) + { + if (landmark.IsObservedInFrame(frame2.GetID()) && landmark.IsObservedInFrame(frame1.GetID())) + { + const data::Feature *feat1 = landmark.GetObservationByFrameID(frame1.GetID()); + const data::Feature *feat2 = landmark.GetObservationByFrameID(frame2.GetID()); + x1.push_back(feat1->GetBearing().normalized()); + x2.push_back(feat2->GetBearing().normalized()); + indexToLandmarkIndex[x1.size() - 1] = i; + } + i++; + } + int inliers = RANSAC(x1, x2, E); + std::vector indices = GetInlierIndices(x1, x2, E); + inlier_indices.clear(); + inlier_indices.reserve(indices.size()); + for (int inx : indices) + { + inlier_indices.push_back(indexToLandmarkIndex[inx]); + } + return inliers; +} + +int FivePoint::Compute(const std::vector &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E) const +{ + std::vector temp; + return Compute(landmarks, frame1, frame2, E, temp); +} + +int FivePoint::RANSAC(const std::vector &x1, const std::vector &x2, Matrix3d &E) const +{ + int maxInliers = 0; + #pragma omp parallel for + for (int i = 0; i < ransacIterations_; i++) + { + std::random_device rd; + std::mt19937 eng(rd()); + std::uniform_int_distribution<> distr(0, x1.size() - 1); + std::set randset; + while (randset.size() < 5) + { + randset.insert(distr(eng)); + } + std::vector indices; + std::copy(randset.begin(), randset.end(), std::back_inserter(indices)); + + std::vector iterE; + std::vector x1s(5); + std::vector x2s(5); + for (int j = 0; j < 5; j++) + { + x1s[j] = x1[indices[j]]; + x2s[j] = x2[indices[j]]; + } + FivePointRelativePose(x1s, x2s, iterE); + + for (Matrix3d &e : iterE) + { + int inliers = GetInlierIndices(x1, x2, e).size(); + #pragma omp critical + { + if (inliers > maxInliers) + { + maxInliers = inliers; + E = e; + } + } + } + } + return maxInliers; +} + +std::vector FivePoint::GetInlierIndices(const std::vector &x1, const std::vector &x2, const Matrix3d &E) const +{ + std::vector indices; + for (int i = 0; i < x1.size(); i++) + { + RowVector3d epiplane1 = x2[i].transpose() * E; + epiplane1.normalize(); + double epiErr1 = std::abs(epiplane1 * x1[i]); + Vector3d epiplane2 = E * x1[i]; + epiplane2.normalize(); + double epiErr2 = std::abs(x2[i].transpose() * epiplane2); + if (epiErr1 < epipolarThreshold_ && epiErr2 < epipolarThreshold_) + { + indices.push_back(i); + } + } + return indices; +} + +void FivePoint::FivePointRelativePose(const std::vector &x1, const std::vector &x2, std::vector &Es) const +{ + MatrixXd epipolarConstraint(x1.size(), 9); + for (int i = 0; i < x1.size(); i++) + { + epipolarConstraint.row(i) << x2[i](0) * x1[i].transpose(), x2[i](1) * x1[i].transpose(), x2[i](2) * x1[i].transpose(); + } + Eigen::SelfAdjointEigenSolver solver(epipolarConstraint.transpose() * epipolarConstraint); + Matrix basis = solver.eigenvectors().leftCols<4>(); + Matrix E[3][3] = { + basis.row(0), basis.row(3), basis.row(6), + basis.row(1), basis.row(4), basis.row(7), + basis.row(2), basis.row(5), basis.row(8) + }; + + Matrix EET[3][3]; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + EET[i][j] = 2 * (util::MathUtil::MultiplyDegOnePoly(E[i][0], E[j][0]) + util::MathUtil::MultiplyDegOnePoly(E[i][1], E[j][1]) + util::MathUtil::MultiplyDegOnePoly(E[i][2], E[j][2])); + } + } + Matrix trace = EET[0][0] + EET[1][1] + EET[2][2]; + Matrix traceConstraint; + for (int i = 0; i < 3; i++) + { + for (int j = 0; j < 3; j++) + { + traceConstraint.row(3 * i + j) = util::MathUtil::MultiplyDegTwoDegOnePoly(EET[i][0], E[0][j]) + util::MathUtil::MultiplyDegTwoDegOnePoly(EET[i][1], E[1][j]) + util::MathUtil::MultiplyDegTwoDegOnePoly(EET[i][2], E[2][j]) - 0.5 * util::MathUtil::MultiplyDegTwoDegOnePoly(trace, E[i][j]); + } + } + Matrix determinantConstraint = util::MathUtil::MultiplyDegTwoDegOnePoly(Matrix(util::MathUtil::MultiplyDegOnePoly(E[0][1], E[1][2]) - util::MathUtil::MultiplyDegOnePoly(E[0][2], E[1][1])), E[2][0]) + + util::MathUtil::MultiplyDegTwoDegOnePoly(Matrix(util::MathUtil::MultiplyDegOnePoly(E[0][2], E[1][0]) - util::MathUtil::MultiplyDegOnePoly(E[0][0], E[1][2])), E[2][1]) + + util::MathUtil::MultiplyDegTwoDegOnePoly(Matrix(util::MathUtil::MultiplyDegOnePoly(E[0][0], E[1][1]) - util::MathUtil::MultiplyDegOnePoly(E[0][1], E[1][0])), E[2][2]); + Matrix constraints; + constraints.block<9, 20>(0, 0) = traceConstraint; + constraints.row(9) = determinantConstraint; + + Eigen::FullPivLU> LU(constraints.block<10, 10>(0, 0)); + Matrix elim = LU.solve(constraints.block<10, 10>(0, 10)); + + Matrix action = Matrix::Zero(); + action.block<3, 10>(0, 0) = elim.block<3, 10>(0, 0); + action.row(3) = elim.row(4); + action.row(4) = elim.row(5); + action.row(5) = elim.row(7); + action(6, 0) = -1.0; + action(7, 1) = -1.0; + action(8, 3) = -1.0; + action(9, 6) = -1.0; + + Eigen::EigenSolver> eigensolver(action); + const auto &eigenvectors = eigensolver.eigenvectors(); + const auto &eigenvalues = eigensolver.eigenvalues(); + + Es.reserve(10); + for (int i = 0; i < 10; i++) + { + if (eigenvalues(i).imag() != 0) + { + continue; + } + Matrix3d EMat; + Eigen::Map>(EMat.data()) = basis * eigenvectors.col(i).tail<4>().real(); + Es.emplace_back(EMat.transpose()); + } +} + + +} +} diff --git a/src/odometry/five_point.h b/src/odometry/five_point.h new file mode 100644 index 0000000..86f1532 --- /dev/null +++ b/src/odometry/five_point.h @@ -0,0 +1,35 @@ +#ifndef _FIVE_POINT_H_ +#define _FIVE_POINT_H_ + +#include +#include "data/landmark.h" + +using namespace Eigen; + +namespace omni_slam +{ +namespace odometry +{ + +class FivePoint +{ +public: + FivePoint(int ransac_iterations, double epipolar_threshold); + + int Compute(const std::vector &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E, std::vector &inlier_indices) const; + int Compute(const std::vector &landmarks, const data::Frame &frame1, const data::Frame &frame2, Matrix3d &E) const; + +private: + int RANSAC(const std::vector &x1, const std::vector &x2, Matrix3d &E) const; + void FivePointRelativePose(const std::vector &x1, const std::vector &x2, std::vector &Es) const; + std::vector GetInlierIndices(const std::vector &x1, const std::vector &x2, const Matrix3d &E) const; + + int ransacIterations_; + double epipolarThreshold_; + +}; + +} +} + +#endif /* _FIVE_POINT_H_ */ diff --git a/src/odometry/pnp.cc b/src/odometry/pnp.cc index 7de3d35..9c93aa3 100644 --- a/src/odometry/pnp.cc +++ b/src/odometry/pnp.cc @@ -23,13 +23,15 @@ PNP::PNP(int ransac_iterations, double reprojection_threshold, int num_refine_th { } -int PNP::Compute(const std::vector &landmarks, data::Frame &frame) const +int PNP::Compute(const std::vector &landmarks, data::Frame &frame, std::vector &inlier_indices) const { std::vector xs; std::vector yns; std::vector ys; std::vector features; std::map indexToId; + std::map indexToLandmarkIndex; + int i = 0; for (const data::Landmark &landmark : landmarks) { if (landmark.IsObservedInFrame(frame.GetID())) @@ -52,8 +54,10 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram yns.push_back(pix); ys.push_back(feat->GetBearing()); features.push_back(feat); + indexToLandmarkIndex[xs.size() - 1] = i; indexToId[xs.size() - 1] = landmark.GetID(); } + i++; } Matrix pose; int inliers = RANSAC(xs, ys, yns, frame.GetCameraModel(), pose); @@ -64,14 +68,23 @@ int PNP::Compute(const std::vector &landmarks, data::Frame &fram } std::vector inlierIds; inlierIds.reserve(indices.size()); + inlier_indices.clear(); + inlier_indices.reserve(indices.size()); for (int inx : indices) { inlierIds.push_back(indexToId[inx]); + inlier_indices.push_back(indexToLandmarkIndex[inx]); } frame.SetEstimatedInversePose(pose, inlierIds); return inliers; } +int PNP::Compute(const std::vector &landmarks, data::Frame &frame) const +{ + std::vector temp; + return Compute(landmarks, frame, temp); +} + int PNP::RANSAC(const std::vector &xs, const std::vector &ys, const std::vector &yns, const camera::CameraModel<> &camera_model, Matrix &pose) const { int maxInliers = 0; diff --git a/src/odometry/pnp.h b/src/odometry/pnp.h index 1c1b0b3..4c3f2aa 100644 --- a/src/odometry/pnp.h +++ b/src/odometry/pnp.h @@ -18,6 +18,7 @@ class PNP { public: PNP(int ransac_iterations, double reprojection_threshold, int num_refine_threads = 1); + int Compute(const std::vector &landmarks, data::Frame &frame, std::vector &inlier_indices) const; int Compute(const std::vector &landmarks, data::Frame &frame) const; private: diff --git a/src/ros/tracking_eval.cc b/src/ros/tracking_eval.cc index 604a979..be3fa32 100644 --- a/src/ros/tracking_eval.cc +++ b/src/ros/tracking_eval.cc @@ -2,6 +2,7 @@ #include "feature/tracker.h" #include "feature/detector.h" +#include "odometry/five_point.h" using namespace std; @@ -16,6 +17,8 @@ TrackingEval::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle string detectorType; int trackerWindowSize; int trackerNumScales; + double fivePointThreshold; + int fivePointRansacIterations; double trackerDeltaPixelErrorThresh; double trackerErrorThresh; map detectorParams; @@ -24,6 +27,8 @@ TrackingEval::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle nhp_.param("detector_type", detectorType, string("GFTT")); nhp_.param("tracker_window_size", trackerWindowSize, 128); nhp_.param("tracker_num_scales", trackerNumScales, 4); + nhp_.param("tracker_checker_epipolar_threshold", fivePointThreshold, 0.01745240643); + nhp_.param("tracker_checker_iterations", fivePointRansacIterations, 1000); nhp_.param("tracker_delta_pixel_error_threshold", trackerDeltaPixelErrorThresh, 5.0); nhp_.param("tracker_error_threshold", trackerErrorThresh, 20.); nhp_.param("min_features_per_region", minFeaturesRegion, 5); @@ -41,7 +46,9 @@ TrackingEval::TrackingEval(const ::ros::NodeHandle &nh, const ::ros::NodeHandle unique_ptr tracker(new feature::Tracker(trackerWindowSize, trackerNumScales, trackerDeltaPixelErrorThresh, trackerErrorThresh)); - trackingModule_.reset(new module::TrackingModule(detector, tracker, minFeaturesRegion)); + unique_ptr checker(new odometry::FivePoint(fivePointRansacIterations, fivePointThreshold)); + + trackingModule_.reset(new module::TrackingModule(detector, tracker, checker, minFeaturesRegion)); } void TrackingEval::InitPublishers() diff --git a/src/util/math_util.h b/src/util/math_util.h index 83c5ea0..970bad6 100644 --- a/src/util/math_util.h +++ b/src/util/math_util.h @@ -1,6 +1,9 @@ #ifndef _MATH_UTIL_H_ #define _MATH_UTIL_H_ +#include +using namespace Eigen; + namespace omni_slam { namespace util @@ -61,6 +64,50 @@ inline bool Roots(T b, T c, T &r1, T &r2) return true; } +template +inline Matrix MultiplyDegOnePoly(const Matrix &a, const Matrix &b) +{ + Matrix output; + output(0) = a(0) * b(0); + output(1) = a(0) * b(1) + a(1) * b(0); + output(2) = a(1) * b(1); + output(3) = a(0) * b(2) + a(2) * b(0); + output(4) = a(1) * b(2) + a(2) * b(1); + output(5) = a(2) * b(2); + output(6) = a(0) * b(3) + a(3) * b(0); + output(7) = a(1) * b(3) + a(3) * b(1); + output(8) = a(2) * b(3) + a(3) * b(2); + output(9) = a(3) * b(3); + return output; +} + +template +inline Matrix MultiplyDegTwoDegOnePoly(const Matrix &a, const Matrix &b) +{ + Matrix output; + output(0) = a(0) * b(0); + output(1) = a(0) * b(1) + a(1) * b(0); + output(2) = a(1) * b(1) + a(2) * b(0); + output(3) = a(2) * b(1); + output(4) = a(0) * b(2) + a(3) * b(0); + output(5) = a(1) * b(2) + a(3) * b(1) + a(4) * b(0); + output(6) = a(2) * b(2) + a(4) * b(1); + output(7) = a(3) * b(2) + a(5) * b(0); + output(8) = a(4) * b(2) + a(5) * b(1); + output(9) = a(5) * b(2); + output(10) = a(0) * b(3) + a(6) * b(0); + output(11) = a(1) * b(3) + a(6) * b(1) + a(7) * b(0); + output(12) = a(2) * b(3) + a(7) * b(1); + output(13) = a(3) * b(3) + a(6) * b(2) + a(8) * b(0); + output(14) = a(4) * b(3) + a(7) * b(2) + a(8) * b(1); + output(15) = a(5) * b(3) + a(8) * b(2); + output(16) = a(6) * b(3) + a(9) * b(0); + output(17) = a(7) * b(3) + a(9) * b(1); + output(18) = a(8) * b(3) + a(9) * b(2); + output(19) = a(9) * b(3); + return output; +} + } } }