From 9936fd3fbe24bbb857795e2de2b9a89892bfca6d Mon Sep 17 00:00:00 2001 From: Viktor Walter Date: Mon, 30 Mar 2020 14:57:33 +0200 Subject: [PATCH 1/2] Adding functions for my ICP implementation --- CMakeLists.txt | 12 ++ include/mrs_lib/icp.h | 27 +++ src/icp/icp.cpp | 374 ++++++++++++++++++++++++++++++++++++++++++ src/icp/test_icp.cpp | 37 +++++ 4 files changed, 450 insertions(+) create mode 100644 include/mrs_lib/icp.h create mode 100644 src/icp/icp.cpp create mode 100644 src/icp/test_icp.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e1c0877d..1fcc3a9a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -229,3 +229,15 @@ target_link_libraries(attitude_converter_test ${catkin_LIBRARIES} ${Eigen_LIBRARIES} ) + +add_library(icp src/icp/icp.cpp) +target_link_libraries(icp + ${catkin_LIBRARIES} + ${Eigen_LIBRARIES} + ) + +add_executable(icp_test src/icp/icp.cpp src/icp/test_icp.cpp) +target_link_libraries(icp_test + ${catkin_LIBRARIES} + ${Eigen_LIBRARIES} + ) diff --git a/include/mrs_lib/icp.h b/include/mrs_lib/icp.h new file mode 100644 index 00000000..f07ef81e --- /dev/null +++ b/include/mrs_lib/icp.h @@ -0,0 +1,27 @@ +#include +#include + +namespace e = Eigen; +namespace icp { + class ICPSolver { + public: + static std::vector> copyShape(std::vector> shape); + static e::Vector2d flatAverage(std::vector input); + static e::Vector3d fullAverage(std::vector input); + static double totalError(std::vector> ref_shape, std::vector input); + static double optimizeRotation(std::vector input, std::vector> shape, e::Vector2d center); + static double optimizeFull(std::vector input, std::vector> shape, e::Vector2d ¢er, e::Vector3d &gradient); + static e::Vector3d matchSquare(std::vector input, double side, e::Vector3d &gradient); + static e::Vector3d matchTriangle(std::vector input, double side, e::Vector3d &gradient); + static e::Vector3d matchLine(std::vector input, double side, e::Vector3d &gradient); + private: + static std::vector> translateShape(std::vector> shape, e::Vector2d pos); + static std::vector> rotateShape(std::vector> shape, e::Vector2d center, double angle); + static double distSquarePointSegment(e::Vector2d point, std::pair segment); + static std::vector> initializeYaw(std::vector input, std::vector> ref_shape, double yaw_step, e::Vector2d center, double& yaw_best); + static std::vector> makeSquare(double side); + static std::vector> makeSquareChamfered(double side); + static std::vector> makeTriangle(double side); + static std::vector> makeLine(double side); + }; +} diff --git a/src/icp/icp.cpp b/src/icp/icp.cpp new file mode 100644 index 00000000..1fde8cad --- /dev/null +++ b/src/icp/icp.cpp @@ -0,0 +1,374 @@ +#include "mrs_lib/icp.h" + +e::Vector3d icp::ICPSolver::matchSquare(std::vector input, double side, e::Vector3d &gradient){ + std::vector> square = makeSquare(side); + std::vector input_2d; + e::Vector2d p_2d; + for (auto p : input){ + p_2d.x() = p.x(); + p_2d.y() = p.y(); + input_2d.push_back(p_2d); + } + e::Vector2d center = flatAverage(input_2d); + /* double yaw = optimizeRotation(input_2d,square, center); */ + double yaw = optimizeFull(input_2d,square, center, gradient); + /* ROS_INFO_STREAM("e " << yaw); */ + + /* center = optimizeTranslation(square); *///TODO + return e::Vector3d(center.x(), center.y(), yaw); +} +//} + +/* matchTriangle() //{ */ +e::Vector3d icp::ICPSolver::matchTriangle(std::vector input, double side, e::Vector3d &gradient){ + std::vector> triangle = makeTriangle(side); + std::vector input_2d; + e::Vector2d p_2d; + for (auto p : input){ + p_2d.x() = p.x(); + p_2d.y() = p.y(); + input_2d.push_back(p_2d); + } + e::Vector2d center = flatAverage(input_2d); + /* double yaw = optimizeRotation(input_2d,square, center); */ + double yaw = optimizeFull(input_2d,triangle, center, gradient); + /* ROS_INFO_STREAM("e " << yaw); */ + + /* center = optimizeTranslation(square); *///TODO + return e::Vector3d(center.x(), center.y(), yaw); +} +//} + +/* matchLine() //{ */ +e::Vector3d icp::ICPSolver::matchLine(std::vector input, double side, e::Vector3d &gradient){ + std::vector> triangle = makeLine(side); + std::vector input_2d; + e::Vector2d p_2d; + for (auto p : input){ + p_2d.x() = p.x(); + p_2d.y() = p.y(); + input_2d.push_back(p_2d); + } + e::Vector2d center = flatAverage(input_2d); + /* double yaw = optimizeRotation(input_2d,square, center); */ + double yaw = optimizeFull(input_2d,triangle, center, gradient); + /* ROS_INFO_STREAM("e " << yaw); */ + + /* center = optimizeTranslation(square); *///TODO + return e::Vector3d(center.x(), center.y(), yaw); +} + +std::vector> icp::ICPSolver::makeSquare(double side){ + double half = side/2.0; + std::vector> output; + std::pair one_side; + one_side.first = e::Vector2d(half, half); + one_side.second = e::Vector2d(half, -half); + output.push_back(one_side); + one_side.first = e::Vector2d(half, -half); + one_side.second = e::Vector2d(-half, -half); + output.push_back(one_side); + one_side.first = e::Vector2d(-half, -half); + one_side.second = e::Vector2d(-half, half); + output.push_back(one_side); + one_side.first = e::Vector2d(-half, half); + one_side.second = e::Vector2d(half, half); + output.push_back(one_side); + return output; +} +std::vector> icp::ICPSolver::makeSquareChamfered(double side){ + double half = side/2.0; + double end = 0.25; + std::vector> output; + std::pair one_side; + one_side.first = e::Vector2d(half, end); + one_side.second = e::Vector2d(half, -end); + output.push_back(one_side); + one_side.first = e::Vector2d(end, -half); + one_side.second = e::Vector2d(-end, -half); + output.push_back(one_side); + one_side.first = e::Vector2d(-half, -end); + one_side.second = e::Vector2d(-half, end); + output.push_back(one_side); + one_side.first = e::Vector2d(-end, half); + one_side.second = e::Vector2d(end, half); + output.push_back(one_side); + + one_side.first = e::Vector2d(end, half); + one_side.second = e::Vector2d(half, end); + output.push_back(one_side); + one_side.first = e::Vector2d(half, -end); + one_side.second = e::Vector2d(end, -half); + output.push_back(one_side); + one_side.first = e::Vector2d(-end, -half); + one_side.second = e::Vector2d(-half, -end); + output.push_back(one_side); + one_side.first = e::Vector2d(-half, end); + one_side.second = e::Vector2d(-end, half); + output.push_back(one_side); + return output; +} +std::vector> icp::ICPSolver::makeTriangle(double side){ + double half = side/2.0; + double top = 0.329423; + std::vector> output; + std::pair one_side; + one_side.first = e::Vector2d(-half, -half); + one_side.second = e::Vector2d(top, 0); + output.push_back(one_side); + one_side.first = e::Vector2d(-half, half); + one_side.second = e::Vector2d(top, 0); + output.push_back(one_side); + one_side.first = e::Vector2d(-half, -half); + one_side.second = e::Vector2d(-half, half); + output.push_back(one_side); + return output; +} +std::vector> icp::ICPSolver::makeLine(double side){ + double half = side/2.0; + double top = 0.329423; + std::vector> output; + std::pair one_side; + one_side.first = e::Vector2d(0, -half); + one_side.second = e::Vector2d(0, half); + output.push_back(one_side); + return output; +} + +e::Vector2d icp::ICPSolver::flatAverage(std::vector input){ + e::Vector2d sum(0,0); + for (auto p : input){ + sum +=p; + } + sum /= (double)(input.size()); + return sum; +} + +e::Vector3d icp::ICPSolver::fullAverage(std::vector input){ + e::Vector3d sum(0,0,0); + for (auto p : input){ + sum +=p; + } + sum /= (double)(input.size()); + return sum; +} + +double icp::ICPSolver::optimizeRotation(std::vector input, std::vector> shape, e::Vector2d center){ + std::vector> shape_shifted = translateShape(shape, center); + double yaw_step = 5.0*(M_PI/180.0); + double error_total = totalError(shape_shifted, input); + std::vector> shape_left, shape_right; + double gradient = std::numeric_limits::max(); + double yaw_curr = 0; + shape_shifted = initializeYaw(input, shape_shifted, 15*(M_PI/180.0), center, yaw_curr); + double threshold = (int)(input.size())*0.001; + int iters = 0; + while ((error_total > threshold) && (fabs(gradient) > 0.0001) && (iters < 50)){ + shape_left = rotateShape(shape_shifted, center, yaw_step); + shape_right = rotateShape(shape_shifted, center, -yaw_step); + double left_error = totalError(shape_left, input); + double right_error = totalError(shape_right, input); + gradient = (left_error-right_error)/(2*yaw_step)/((double)(input.size())); + double yaw_diff = -gradient; + yaw_curr = yaw_curr+yaw_diff; + shape_shifted = rotateShape(shape_shifted, center, yaw_diff); + error_total = totalError(shape_shifted, input); + yaw_step = fabs(yaw_diff/2); + iters++; + ROS_INFO_STREAM("[FireLoclaize]: icpish: yaw: "<< yaw_curr << " error: " << error_total << " gradient: "<< gradient << " from " << left_error << " and " << right_error); + } + ROS_INFO_STREAM("[FireLoclaize]: icpish final: yaw: "<< yaw_curr << " error: " << error_total << " gradient: "<< gradient); + return yaw_curr; +} + +double icp::ICPSolver::optimizeFull(std::vector input, std::vector> shape, e::Vector2d ¢er, e::Vector3d &gradient){ + std::vector> shape_shifted = translateShape(shape, center); + double yaw_step = 5.0*(M_PI/180.0); + double x_step = 0.1; + double y_step = 0.1; + double error_total = totalError(shape_shifted, input); + std::vector> shape_ccv, shape_cv; + std::vector> shape_left, shape_right; + std::vector> shape_front, shape_back; + double ccv_error, cv_error; + double left_error, right_error; + double front_error, back_error; + gradient = e::Vector3d ( + std::numeric_limits::max(), + std::numeric_limits::max(), + std::numeric_limits::max() + ); + double yaw_curr = 0; + shape_shifted = initializeYaw(input, shape_shifted, 15*(M_PI/180.0), center, yaw_curr); + double threshold = (int)(input.size())*0.001; + int iters = 0; + while ((error_total > threshold) && ((gradient.norm()) > 0.0001) && (iters < 50)){ + shape_ccv = rotateShape(shape_shifted, center, yaw_step); + shape_cv = rotateShape(shape_shifted, center, -yaw_step); + shape_left = translateShape(shape_shifted, e::Vector2d(0,y_step)); + shape_right = translateShape(shape_shifted, e::Vector2d(0,-y_step)); + shape_front = translateShape(shape_shifted, e::Vector2d(x_step,0)); + shape_back = translateShape(shape_shifted, e::Vector2d(-x_step,0)); + ccv_error = totalError(shape_ccv, input); + cv_error = totalError(shape_cv, input); + left_error = totalError(shape_left, input); + right_error = totalError(shape_right, input); + front_error = totalError(shape_front, input); + back_error = totalError(shape_back, input); + gradient.z() = ((ccv_error-cv_error)/(2*yaw_step))/((double)(input.size())); + gradient.y() = ((left_error-right_error)/(2*y_step))/((double)(input.size())); + gradient.x() = ((front_error-back_error)/(2*x_step))/((double)(input.size())); + double yaw_diff, y_diff, x_diff; + if (gradient.z() > 1E-9) + yaw_diff = -gradient.z(); + else + yaw_diff = 0; + if (gradient.y() > 1E-9) + y_diff = -gradient.y(); + else + y_diff = 0; + if (gradient.x() > 1E-9) + x_diff = -gradient.x(); + else + x_diff = 0; + yaw_curr = yaw_curr+yaw_diff; + center = center+e::Vector2d(x_diff,y_diff); + shape_shifted = rotateShape(translateShape(shape_shifted, e::Vector2d(x_diff, y_diff)), center, yaw_diff); + error_total = totalError(shape_shifted, input); + yaw_step = fabs(yaw_diff/2); + iters++; + /* ROS_INFO_STREAM("[FireLoclaize]: icpish: yaw: "<< yaw_curr << " center: " << center.transpose() << " error: " << error_total << " gradient: "<< gradient.transpose()); */ + } + //final gradient check + double mean_error = error_total/((double)(input.size())); + yaw_step = 5.0*(M_PI/180.0); + x_step = 0.10; + y_step = 0.10; + shape_ccv = rotateShape(shape_shifted, center, yaw_step); + shape_cv = rotateShape(shape_shifted, center, -yaw_step); + shape_left = translateShape(shape_shifted, e::Vector2d(0,y_step)); + shape_right = translateShape(shape_shifted, e::Vector2d(0,-y_step)); + shape_front = translateShape(shape_shifted, e::Vector2d(x_step,0)); + shape_back = translateShape(shape_shifted, e::Vector2d(-x_step,0)); + gradient.z() = ((((abs(ccv_error)+abs(cv_error))/2))/(yaw_step))/((double)(input.size())); + gradient.y() = ((((abs(left_error)+abs(right_error))/2)-mean_error)/(y_step))/((double)(input.size())); + gradient.x() = ((((abs(front_error)+abs(back_error))/2)-mean_error)/(x_step))/((double)(input.size())); + /* ROS_INFO_STREAM("[FireLoclaize]: icpish: yaw: "<< yaw_curr << " center: " << center.transpose() << " error: " << error_total << " gradient: "<< gradient.transpose()); */ + /* ROS_INFO_STREAM("e " << yaw); */ + return yaw_curr; +} + +double icp::ICPSolver::totalError(std::vector> ref_shape, std::vector input){ + double error = 0; + std::vector side_errors; + for (int i=0; i<(int)(input.size()); i++){ + double best_side_error = std::numeric_limits::max(); + int best_side = -1; + for (int j=0; j<(int)(ref_shape.size()); j++){ + double curr_side_error = distSquarePointSegment(input[i], ref_shape[j]); + side_errors.push_back(curr_side_error); + /* ROS_INFO_STREAM("curr_side_error: " << curr_side_error); */ + if (curr_side_error < best_side_error){ + best_side_error = curr_side_error; + best_side = j; + } + + } + /* if (best_side >= 0) */ + /* ROS_INFO_STREAM("bse: " << best_side_error << " P: " << input[i].transpose() << " s: " << ref_shape[best_side].first.transpose() << " : " << ref_shape[best_side].second.transpose()); */ + /* else */ + /* ROS_INFO_STREAM("bse: not found!"); */ + error += best_side_error; + } + if (error > 1E100){ + ROS_INFO_STREAM("TOTAL ERROR: " << error); + ROS_INFO_STREAM("side errors: " << side_errors.size()); + for (auto s : side_errors) + ROS_INFO_STREAM("s: " << s); + ROS_INFO_STREAM("input: " << input.size()); + for (auto i : input) + ROS_INFO_STREAM("i: " << i); + ROS_INFO_STREAM("ref_shape size: " << ref_shape.size();); + } + return error; +} + +double icp::ICPSolver::distSquarePointSegment(e::Vector2d point, std::pair segment){ + // Return minimum distance between line segment vw and point p + float l2 = (segment.first - segment.second).squaredNorm(); // i.e. |w-v|^2 - avoid a sqrt + if (l2 == 0.0) return (segment.first - point).squaredNorm(); // v == w case + // Consider the line extending the segment, parameterized as v + t (w - v). + // We find projection of point p onto the line. + // It falls where t = [(p-v) . (w-v)] / |w-v|^2 + // We clamp t from [0,1] to handle points outside the segment vw. + float t = ((point - segment.first).dot(segment.second - segment.first)) / l2; + if (std::isnan(t)) + ROS_INFO_STREAM("t: " << t<< " : " << point.transpose() << " : "<< segment.first.transpose()<< " : " << segment.second.transpose() << " : " << l2); + if ( t < 0 ){ + /* if (((point - segment.first).squaredNorm() > 10000) || std::isnan((point - segment.first).squaredNorm())) */ + /* ROS_INFO_STREAM("a: " << segment.first.transpose() << ":" << point.transpose()); */ + return (point - segment.first).squaredNorm(); + } else if ( t > 1 ){ + /* if (((point - segment.second).squaredNorm() > 10000) || (std::isnan((point - segment.second).squaredNorm()))) */ + /* ROS_INFO_STREAM("b: " << segment.second.transpose() << ":" << point.transpose()); */ + return (point - segment.second).squaredNorm(); + } else { + e::Vector2d projection = segment.first + t * (segment.second - segment.first); // Projection falls on the segment + if (((point - projection).squaredNorm() > 10000) || (std::isnan((point - projection).squaredNorm()))) + ROS_INFO_STREAM("c: " << projection.transpose() << ":" << point.transpose()); + return (point - projection).squaredNorm(); + } +} + +std::vector> icp::ICPSolver::copyShape(std::vector> shape){ + std::vector> output; + std::pair one_side; + for (auto side : shape){ + one_side.first = side.first; + one_side.second = side.second; + output.push_back(one_side); + } + return output; +} + +std::vector> icp::ICPSolver::initializeYaw(std::vector input, std::vector> ref_shape, double yaw_step, e::Vector2d center, double& yaw_best){ + int step_count = ceil((M_PI*2)/yaw_step) + 1; + double yaw_curr = yaw_best; + double error_best = std::numeric_limits::max(); + double error_curr; + std::vector> shape_best = copyShape(ref_shape); + std::vector> shape_curr = copyShape(ref_shape); + for (int i=0; i> icp::ICPSolver::translateShape(std::vector> shape, e::Vector2d pos){ + std::vector> output; + std::pair one_side; + for (auto side : shape){ + one_side.first = side.first+pos; + one_side.second = side.second+pos; + output.push_back(one_side); + } + return output; +} +std::vector> icp::ICPSolver::rotateShape(std::vector> shape, e::Vector2d center, double angle){ + std::vector> output; + std::pair one_side; + e::Rotation2D R(-angle); //yaw is ccw + for (auto side : shape){ + one_side.first = R*(side.first-center)+center; + one_side.second = R*(side.second-center)+center; + output.push_back(one_side); + } + return output; +} diff --git a/src/icp/test_icp.cpp b/src/icp/test_icp.cpp new file mode 100644 index 00000000..83468c30 --- /dev/null +++ b/src/icp/test_icp.cpp @@ -0,0 +1,37 @@ +#include "mrs_lib/icp.h" +#include + +void fixSymmetry(e::Vector3d &input){ + input.z() = fmod(input.z(),M_PI_2); + if (fabs(input.z())>(M_PI_2/2.0)){ + if (input.z()>0) + input.z() -= M_PI_2; + else + input.z() += M_PI_2; + } +} + +int main(int argc, char** argv){ + e::Vector3d gradient; + std::vector square_points; + + square_points.push_back(e::Vector3d(0.0,0.1,1.0)); + square_points.push_back(e::Vector3d(0.9,-0.1,1.2)); + square_points.push_back(e::Vector3d(1.1,1.0,0.9)); + square_points.push_back(e::Vector3d(-0.1,0.9,1.0)); + auto square_result = icp::ICPSolver::matchSquare(square_points, 1.0, gradient); + fixSymmetry(square_result); + std::cout << "aligned square result: " << square_result.transpose() << std::endl; + + square_points.clear(); + square_points.push_back(e::Vector3d(0.5,-0.2,1.0)); + square_points.push_back(e::Vector3d(1.2,0.5,1.0)); + square_points.push_back(e::Vector3d(0.5,1.2,1.0)); + square_points.push_back(e::Vector3d(-0.2,0.5,1.0)); + square_result.z() = fmod(square_result.z(),M_PI_2); + square_result = icp::ICPSolver::matchSquare(square_points, 1.0, gradient); + fixSymmetry(square_result); + std::cout << "diagonal square result: " << square_result.transpose() << std::endl; + + return 0; + } From b541832a67975975cc9181e5d7549509833e0cf4 Mon Sep 17 00:00:00 2001 From: Viktor Walter Date: Mon, 30 Mar 2020 19:37:28 +0200 Subject: [PATCH 2/2] Tuned some parameters --- include/mrs_lib/icp.h | 1 + src/icp/icp.cpp | 180 ++++++++++++++++++++++++++++++++---------- src/icp/test_icp.cpp | 29 +++++++ 3 files changed, 170 insertions(+), 40 deletions(-) diff --git a/include/mrs_lib/icp.h b/include/mrs_lib/icp.h index f07ef81e..17da9d9e 100644 --- a/include/mrs_lib/icp.h +++ b/include/mrs_lib/icp.h @@ -11,6 +11,7 @@ namespace icp { static double totalError(std::vector> ref_shape, std::vector input); static double optimizeRotation(std::vector input, std::vector> shape, e::Vector2d center); static double optimizeFull(std::vector input, std::vector> shape, e::Vector2d ¢er, e::Vector3d &gradient); + static e::Vector3d matchShape(std::vector input, std::vector> shape, e::Vector3d &gradient); static e::Vector3d matchSquare(std::vector input, double side, e::Vector3d &gradient); static e::Vector3d matchTriangle(std::vector input, double side, e::Vector3d &gradient); static e::Vector3d matchLine(std::vector input, double side, e::Vector3d &gradient); diff --git a/src/icp/icp.cpp b/src/icp/icp.cpp index 1fde8cad..d908b660 100644 --- a/src/icp/icp.cpp +++ b/src/icp/icp.cpp @@ -1,5 +1,36 @@ #include "mrs_lib/icp.h" +static double signedCompare(double a, double b){ +/* double signedCompare(double a, double b){ */ + if (fabs(a) < fabs(b)){ + /* ROS_INFO_STREAM("yes"); */ + return a; + } + else { + /* ROS_INFO_STREAM("no"); */ + return ((a>0?1.0:-1.0)*fabs(b)); + } +} + +/* matchShape() //{ */ +e::Vector3d icp::ICPSolver::matchShape(std::vector input, std::vector> shape, e::Vector3d &gradient){ + std::vector input_2d; + e::Vector2d p_2d; + for (auto p : input){ + p_2d.x() = p.x(); + p_2d.y() = p.y(); + input_2d.push_back(p_2d); + } + e::Vector2d center = flatAverage(input_2d); + double yaw = optimizeFull(input_2d,shape, center, gradient); + /* ROS_INFO_STREAM("e " << yaw); */ + + /* center = optimizeTranslation(square); *///TODO + return e::Vector3d(center.x(), center.y(), yaw); +} +//} + +/* matchSquare() //{ */ e::Vector3d icp::ICPSolver::matchSquare(std::vector input, double side, e::Vector3d &gradient){ std::vector> square = makeSquare(side); std::vector input_2d; @@ -186,7 +217,6 @@ double icp::ICPSolver::optimizeFull(std::vector input, std::vector< double yaw_step = 5.0*(M_PI/180.0); double x_step = 0.1; double y_step = 0.1; - double error_total = totalError(shape_shifted, input); std::vector> shape_ccv, shape_cv; std::vector> shape_left, shape_right; std::vector> shape_front, shape_back; @@ -200,59 +230,128 @@ double icp::ICPSolver::optimizeFull(std::vector input, std::vector< ); double yaw_curr = 0; shape_shifted = initializeYaw(input, shape_shifted, 15*(M_PI/180.0), center, yaw_curr); - double threshold = (int)(input.size())*0.001; + /* ROS_INFO_STREAM("initialized yaw at: " << yaw_curr); */ + double threshold_err = pow((double)(input.size())*0.0001,2); + double threshold_grad = 0.0001*((double)(input.size())); int iters = 0; - while ((error_total > threshold) && ((gradient.norm()) > 0.0001) && (iters < 50)){ - shape_ccv = rotateShape(shape_shifted, center, yaw_step); - shape_cv = rotateShape(shape_shifted, center, -yaw_step); - shape_left = translateShape(shape_shifted, e::Vector2d(0,y_step)); - shape_right = translateShape(shape_shifted, e::Vector2d(0,-y_step)); - shape_front = translateShape(shape_shifted, e::Vector2d(x_step,0)); - shape_back = translateShape(shape_shifted, e::Vector2d(-x_step,0)); - ccv_error = totalError(shape_ccv, input); - cv_error = totalError(shape_cv, input); - left_error = totalError(shape_left, input); - right_error = totalError(shape_right, input); - front_error = totalError(shape_front, input); - back_error = totalError(shape_back, input); + /* e::Matrix3d normalizer; */ + /* normalizer << */ + /* 1,0,0, */ + /* 0,1,0, */ + /* 0,0,1.146 */ + double error_total = totalError(shape_shifted, input); + bool stop_shrinkage; + while ((error_total > threshold_err) && ((gradient.norm()) > threshold_grad) && (iters < 50)){ + + yaw_step = 5.0*(M_PI/180.0); + x_step = 0.1; + y_step = 0.1; + + stop_shrinkage = false; + do { + shape_ccv = rotateShape(shape_shifted, center, yaw_step); + shape_cv = rotateShape(shape_shifted, center, -yaw_step); + ccv_error = totalError(shape_ccv, input); + cv_error = totalError(shape_cv, input); + if (( (ccv_error-error_total)*(cv_error-error_total) ) > 0){ + yaw_step = 0.5*yaw_step; + } else { + stop_shrinkage = true; + } + + if ((yaw_step < 0.00001) && (!stop_shrinkage)){ + cv_error = ccv_error; + stop_shrinkage = true; + } + /* ROS_INFO_STREAM("yaw_step: " << x_step << " ccv_diff: " << (ccv_error-error_total) << " cv_diff: " << (cv_error-error_total)); */ + } while(!stop_shrinkage); + + stop_shrinkage = false; + do { + shape_left = translateShape(shape_shifted, e::Vector2d(0,y_step)); + shape_right = translateShape(shape_shifted, e::Vector2d(0,-y_step)); + left_error = totalError(shape_left, input); + right_error = totalError(shape_right, input); + if (( (left_error-error_total)*(right_error-error_total) ) > 0){ + y_step = 0.5*y_step; + } else { + stop_shrinkage = true; + } + + if ((y_step < 0.00001) && (!stop_shrinkage)){ + right_error = left_error; + stop_shrinkage = true; + } + /* ROS_INFO_STREAM("y_step: " << x_step << " left_diff: " << (left_error-error_total) << " right_diff: " << (right_error-error_total)); */ + } while(!stop_shrinkage); + + stop_shrinkage = false; + do { + shape_front = translateShape(shape_shifted, e::Vector2d(x_step,0)); + shape_back = translateShape(shape_shifted, e::Vector2d(-x_step,0)); + front_error = totalError(shape_front, input); + back_error = totalError(shape_back, input); + if (( (front_error-error_total)*(back_error-error_total) ) > 0){ + x_step = 0.5*x_step; + } else { + stop_shrinkage = true; + } + + if ((x_step < 0.00001) && (!stop_shrinkage)){ + back_error = front_error; + stop_shrinkage = true; + } + /* ROS_INFO_STREAM("x_step: " << x_step << " front_diff: " << (front_error-error_total) << " back_diff: " << (back_error-error_total)); */ + } while(!stop_shrinkage); + gradient.z() = ((ccv_error-cv_error)/(2*yaw_step))/((double)(input.size())); gradient.y() = ((left_error-right_error)/(2*y_step))/((double)(input.size())); gradient.x() = ((front_error-back_error)/(2*x_step))/((double)(input.size())); double yaw_diff, y_diff, x_diff; - if (gradient.z() > 1E-9) - yaw_diff = -gradient.z(); - else + if (fabs(gradient.z()) > 1E-9) + yaw_diff = -0.5*gradient.z(); + else{ yaw_diff = 0; - if (gradient.y() > 1E-9) - y_diff = -gradient.y(); - else + gradient.z() = 0; + } + if (fabs(gradient.y()) > 1E-9) + y_diff = -0.5*gradient.y(); + else{ y_diff = 0; - if (gradient.x() > 1E-9) - x_diff = -gradient.x(); - else + gradient.y() = 0; + } + if (fabs(gradient.x()) > 1E-9) + x_diff = -0.5*gradient.x(); + else{ x_diff = 0; + gradient.x() = 0; + } + yaw_diff = signedCompare(yaw_diff,yaw_step); + y_diff = signedCompare(y_diff,y_step); + x_diff = signedCompare(x_diff,x_step); + yaw_curr = yaw_curr+yaw_diff; center = center+e::Vector2d(x_diff,y_diff); - shape_shifted = rotateShape(translateShape(shape_shifted, e::Vector2d(x_diff, y_diff)), center, yaw_diff); + shape_shifted = translateShape(rotateShape(shape_shifted, center, yaw_diff), e::Vector2d(x_diff, y_diff)); error_total = totalError(shape_shifted, input); yaw_step = fabs(yaw_diff/2); iters++; - /* ROS_INFO_STREAM("[FireLoclaize]: icpish: yaw: "<< yaw_curr << " center: " << center.transpose() << " error: " << error_total << " gradient: "<< gradient.transpose()); */ + /* ROS_INFO_STREAM("icpish: yaw: "<< yaw_curr << " center: " << center.transpose() << " error: " << error_total << " gradient: "<< gradient.transpose() << " gradient.norm " << gradient.norm() << " total error: " << error_total); */ } //final gradient check - double mean_error = error_total/((double)(input.size())); - yaw_step = 5.0*(M_PI/180.0); - x_step = 0.10; - y_step = 0.10; - shape_ccv = rotateShape(shape_shifted, center, yaw_step); - shape_cv = rotateShape(shape_shifted, center, -yaw_step); - shape_left = translateShape(shape_shifted, e::Vector2d(0,y_step)); - shape_right = translateShape(shape_shifted, e::Vector2d(0,-y_step)); - shape_front = translateShape(shape_shifted, e::Vector2d(x_step,0)); - shape_back = translateShape(shape_shifted, e::Vector2d(-x_step,0)); - gradient.z() = ((((abs(ccv_error)+abs(cv_error))/2))/(yaw_step))/((double)(input.size())); - gradient.y() = ((((abs(left_error)+abs(right_error))/2)-mean_error)/(y_step))/((double)(input.size())); - gradient.x() = ((((abs(front_error)+abs(back_error))/2)-mean_error)/(x_step))/((double)(input.size())); + /* double mean_error = error_total/((double)(input.size())); */ + /* yaw_step = 5.0*(M_PI/180.0); */ + /* x_step = 0.10; */ + /* y_step = 0.10; */ + /* shape_ccv = rotateShape(shape_shifted, center, yaw_step); */ + /* shape_cv = rotateShape(shape_shifted, center, -yaw_step); */ + /* shape_left = translateShape(shape_shifted, e::Vector2d(0,y_step)); */ + /* shape_right = translateShape(shape_shifted, e::Vector2d(0,-y_step)); */ + /* shape_front = translateShape(shape_shifted, e::Vector2d(x_step,0)); */ + /* shape_back = translateShape(shape_shifted, e::Vector2d(-x_step,0)); */ + /* gradient.z() = ((((abs(ccv_error)+abs(cv_error))/2))/(yaw_step))/((double)(input.size())); */ + /* gradient.y() = ((((abs(left_error)+abs(right_error))/2)-mean_error)/(y_step))/((double)(input.size())); */ + /* gradient.x() = ((((abs(front_error)+abs(back_error))/2)-mean_error)/(x_step))/((double)(input.size())); */ /* ROS_INFO_STREAM("[FireLoclaize]: icpish: yaw: "<< yaw_curr << " center: " << center.transpose() << " error: " << error_total << " gradient: "<< gradient.transpose()); */ /* ROS_INFO_STREAM("e " << yaw); */ return yaw_curr; @@ -347,6 +446,7 @@ std::vector> icp::ICPSolver::initializeYaw(st error_best = error_curr; yaw_best = yaw_curr; } + /* ROS_INFO_STREAM("curr yaw: " << yaw_curr << " curr err: " << error_curr); */ } return shape_best; } @@ -364,7 +464,7 @@ std::vector> icp::ICPSolver::translateShape(s std::vector> icp::ICPSolver::rotateShape(std::vector> shape, e::Vector2d center, double angle){ std::vector> output; std::pair one_side; - e::Rotation2D R(-angle); //yaw is ccw + e::Rotation2D R(angle); //yaw is ccw for (auto side : shape){ one_side.first = R*(side.first-center)+center; one_side.second = R*(side.second-center)+center; diff --git a/src/icp/test_icp.cpp b/src/icp/test_icp.cpp index 83468c30..d41e0d58 100644 --- a/src/icp/test_icp.cpp +++ b/src/icp/test_icp.cpp @@ -33,5 +33,34 @@ int main(int argc, char** argv){ fixSymmetry(square_result); std::cout << "diagonal square result: " << square_result.transpose() << std::endl; + std::vector l_points; + l_points.push_back(e::Vector3d(100.0,0.0,1.0)); + l_points.push_back(e::Vector3d(100.0,1.0,1.0)); + l_points.push_back(e::Vector3d(102.0,0.0,1.0)); + std::vector> l_shape; + std::pair one_side; + one_side.first = e::Vector2d(0.0, 0.0); + one_side.second = e::Vector2d(-1.0, 0.0); + l_shape.push_back(one_side); + one_side.first = e::Vector2d(0.0, 0.0); + one_side.second = e::Vector2d(0.0, 2.0); + l_shape.push_back(one_side); + auto l_result = icp::ICPSolver::matchShape(l_points,l_shape,gradient); + std::cout << "l shape result: " << l_result.transpose() << " gradient: " << gradient.transpose() << std::endl; + + l_points.clear(); + l_points.push_back(e::Vector3d(100.0,0.0,1.0)); + l_points.push_back(e::Vector3d(99.826,0.985,1.0)); + l_points.push_back(e::Vector3d(101.970,0.347,1.0)); + l_result = icp::ICPSolver::matchShape(l_points,l_shape,gradient); + std::cout << "rotated l shape result: " << l_result.transpose() << " gradient: " << gradient.transpose() << std::endl; + + l_points.clear(); + l_points.push_back(e::Vector3d(100.1,0.1,1.0)); + l_points.push_back(e::Vector3d(100.0,0.9,1.0)); + l_points.push_back(e::Vector3d(101.9,0.0,1.0)); + l_result = icp::ICPSolver::matchShape(l_points,l_shape,gradient); + std::cout << "deformed l shape result: " << l_result.transpose() << " gradient: " << gradient.transpose() << std::endl; + return 0; }