Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Icp #1

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
Open

Icp #1

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
9936fd3
Adding functions for my ICP implementation
ViktorWalter Mar 30, 2020
b541832
Tuned some parameters
ViktorWalter Mar 30, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
12 changes: 12 additions & 0 deletions CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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}
)
28 changes: 28 additions & 0 deletions include/mrs_lib/icp.h
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,28 @@
#include <Eigen/Dense>
#include <ros/ros.h>

namespace e = Eigen;
namespace icp {
class ICPSolver {
public:
static std::vector<std::pair<e::Vector2d,e::Vector2d>> copyShape(std::vector<std::pair<e::Vector2d,e::Vector2d>> shape);
static e::Vector2d flatAverage(std::vector<e::Vector2d> input);
static e::Vector3d fullAverage(std::vector<e::Vector3d> input);
static double totalError(std::vector<std::pair<e::Vector2d,e::Vector2d>> ref_shape, std::vector<e::Vector2d> input);
static double optimizeRotation(std::vector<e::Vector2d> input, std::vector<std::pair<e::Vector2d,e::Vector2d>> shape, e::Vector2d center);
static double optimizeFull(std::vector<e::Vector2d> input, std::vector<std::pair<e::Vector2d,e::Vector2d>> shape, e::Vector2d &center, e::Vector3d &gradient);
static e::Vector3d matchShape(std::vector<e::Vector3d> input, std::vector<std::pair<e::Vector2d,e::Vector2d>> shape, e::Vector3d &gradient);
static e::Vector3d matchSquare(std::vector<e::Vector3d> input, double side, e::Vector3d &gradient);
static e::Vector3d matchTriangle(std::vector<e::Vector3d> input, double side, e::Vector3d &gradient);
static e::Vector3d matchLine(std::vector<e::Vector3d> input, double side, e::Vector3d &gradient);
private:
static std::vector<std::pair<e::Vector2d,e::Vector2d>> translateShape(std::vector<std::pair<e::Vector2d,e::Vector2d>> shape, e::Vector2d pos);
static std::vector<std::pair<e::Vector2d,e::Vector2d>> rotateShape(std::vector<std::pair<e::Vector2d,e::Vector2d>> shape, e::Vector2d center, double angle);
static double distSquarePointSegment(e::Vector2d point, std::pair<e::Vector2d,e::Vector2d> segment);
static std::vector<std::pair<e::Vector2d,e::Vector2d>> initializeYaw(std::vector<e::Vector2d> input, std::vector<std::pair<e::Vector2d,e::Vector2d>> ref_shape, double yaw_step, e::Vector2d center, double& yaw_best);
static std::vector<std::pair<e::Vector2d,e::Vector2d>> makeSquare(double side);
static std::vector<std::pair<e::Vector2d,e::Vector2d>> makeSquareChamfered(double side);
static std::vector<std::pair<e::Vector2d,e::Vector2d>> makeTriangle(double side);
static std::vector<std::pair<e::Vector2d,e::Vector2d>> makeLine(double side);
};
}
Loading