175 projected_p2d.x = c.projected_p2d.x;
176 projected_p2d.y = c.projected_p2d.y;
186 template <
typename T>
195 DoHandleTest(
int _type_id = -1,
bool _needs_has_p2d =
false,
bool _needs_has_p3d =
false)
196 : type_id(_type_id), needs_has_p2d(_needs_has_p2d), needs_has_p3d(_needs_has_p3d)
200 operator()(
const T &f)
const
202 if (needs_has_p2d && !f.has_p2d)
204 if (needs_has_p3d && !f.has_p3d)
207 if ((type_id != -1) && (type_id != f.type_id))
218 template <
typename T>
223 bool erase_without_p2d;
224 bool erase_without_p3d;
225 bool test_reprojection;
229 DoEraseTest(
bool _erase_without_p2d,
bool _erase_without_p3d,
int _type_id = -1)
231 erase_without_p2d(_erase_without_p2d),
232 erase_without_p3d(_erase_without_p3d),
233 test_reprojection(
false),
238 bool _erase_without_p2d =
false,
239 bool _erase_without_p3d =
false,
242 erase_without_p2d(_erase_without_p2d),
243 erase_without_p3d(_erase_without_p3d),
244 test_reprojection(
true),
245 limit_sq(_limit * _limit)
249 operator()(
const T &f)
const
251 if ((type_id != -1) && (type_id != f.type_id))
253 if (erase_without_p2d && !f.has_p2d)
255 if (erase_without_p3d && !f.has_p3d)
257 if (test_reprojection) {
263 double dist_sq = (f.p2d.x - f.projected_p2d.x) * (f.p2d.x - f.projected_p2d.x);
264 dist_sq += (f.p2d.y - f.projected_p2d.y) * (f.p2d.y - f.projected_p2d.y);
265 if (dist_sq > limit_sq)
273 template <
typename T,
typename F>
278 typename std::map<int, T>::iterator iter = container.begin();
279 while (iter != container.end()) {
281 if (do_erase_test(f)) {
282 container.erase(iter++);
299 int _min_features = 90,
300 double _quality_level = 0.01,
301 double _min_distance = 10,
315 template <
typename T>
319 std::map<int, T> &container,
327 return Track(img, mask, container, do_handle_test_default, type_id, first_id, last_id);
371 template <
typename T,
typename F>
375 std::map<int, T> &container,
385 typename std::map<int, T>::iterator iter = container.begin();
386 typename std::map<int, T>::iterator iter_end = container.end();
388 for (; iter != iter_end; iter++) {
390 if (!do_handle_test(f))
392 if (f.has_p2d !=
true)
411 if (next_id < first_id)
413 if (next_id > last_id)
421 if (last_id >= 0 &&
id >= last_id)
423 T &f = container[id];
432 template <
typename T>
434 AddFeatures(std::map<int, T> &container,
int type_id = 0,
int first_id = 0,
int last_id = 65535)
441 if (last_id >= 0 &&
id >= last_id)
443 T &f = container[id];
452 template <
typename T>
474 Reset(cv::Mat &img, cv::Mat &mask)
479 DelFeature(
int index)
495 template <
typename T>
497 Undistort(std::map<int, T> &container,
int type_id = -1)
500 return Undistort(container, do_handle_test_default);
503 template <
typename T,
typename F>
505 Undistort(std::map<int, T> &container, F &do_handle_test)
507 typename std::map<int, T>::iterator iter = container.begin();
508 typename std::map<int, T>::iterator iter_end = container.end();
509 for (; iter != iter_end; iter++) {
518 template <
typename T>
520 Distort(std::map<int, T> &container,
int type_id = -1)
523 return Distort(container, do_handle_test_default);
526 template <
typename T,
typename F>
528 Distort(std::map<int, T> &container, F &do_handle_test)
530 typename std::map<int, T>::iterator iter = container.begin();
531 typename std::map<int, T>::iterator iter_end = container.end();
532 for (; iter != iter_end; iter++) {
541 template <
typename T>
546 return CalcExteriorOrientation(container, pose, do_handle_test_default);
549 template <
typename T,
typename F>
553 int count_points = container.size();
554 if (count_points < 4)
556 cv::Mat world_points = cv::Mat(count_points, 1, CV_32FC3);
557 cv::Mat image_observations = cv::Mat(count_points * 2, 1, CV_32FC2);
558 typename std::map<int, T>::iterator iter = container.begin();
559 typename std::map<int, T>::iterator iter_end = container.end();
561 for (; iter != iter_end; iter++) {
563 if (!do_handle_test(f))
565 if (f.has_p2d && f.has_p3d) {
566 world_points.at<
float>(ind * 3 + 0) = (
float)f.p3d.x;
567 world_points.at<
float>(ind * 3 + 1) = (
float)f.p3d.y;
568 world_points.at<
float>(ind * 3 + 2) = (
float)f.p3d.z;
569 image_observations.at<
float>(ind * 2 + 0) = (
float)f.p2d.x;
570 image_observations.at<
float>(ind * 2 + 1) = (
float)f.p2d.y;
582 cv::Mat world_points_sub = world_points(r);
583 cv::Mat image_observations_sub = image_observations(r);
587 image_observations.release();
588 world_points.release();
593 template <
typename T>
598 std::map<int, double> *weights = 0)
601 return UpdatePose(container, pose, do_handle_test_default, weights);
604 template <
typename T>
609 return UpdateRotation(container, pose, do_handle_test_default);
612 template <
typename T,
typename F>
616 int count_points = container.size();
617 if (count_points < 6)
620 cv::Mat world_points = cv::Mat(count_points, 1, CV_64FC3);
621 cv::Mat image_observations = cv::Mat(count_points * 2, 1, CV_64F);
624 typename std::map<int, T>::iterator iter = container.begin();
625 typename std::map<int, T>::iterator iter_end = container.end();
626 for (; iter != iter_end; iter++) {
628 if (!do_handle_test(f))
630 if (f.has_p2d && f.has_p3d) {
631 world_points.at<
float>(ind * 3 + 0) = (
float)f.p3d.x;
632 world_points.at<
float>(ind * 3 + 1) = (
float)f.p3d.y;
633 world_points.at<
float>(ind * 3 + 2) = (
float)f.p3d.z;
634 image_observations.at<
float>(ind * 2 + 0) = (
float)f.p2d.x;
635 image_observations.at<
float>(ind * 2 + 1) = (
float)f.p2d.y;
647 cv::Mat world_points_sub = world_points(r);
650 cv::Mat image_observations_sub = image_observations(r);
652 bool ret = UpdateRotation(world_points_sub, image_observations_sub, pose);
654 image_observations.release();
655 world_points.release();
660 template <
typename T,
typename F>
665 std::map<int, double> *weights = 0)
667 int count_points = container.size();
668 if (count_points < 4)
671 cv::Mat world_points = cv::Mat(count_points, 1, CV_64FC3);
672 cv::Mat image_observations = cv::Mat(count_points * 2, 1, CV_64F);
673 cv::Mat w = cv::Mat();
675 w = cv::Mat(count_points * 2, 1, CV_64F);
678 typename std::map<int, T>::iterator iter = container.begin();
679 typename std::map<int, T>::iterator iter_end = container.end();
680 for (; iter != iter_end; iter++) {
682 if (!do_handle_test(f))
684 if (f.has_p2d && f.has_p3d) {
685 world_points.at<
float>(ind * 3 + 0) = (
float)f.p3d.x;
686 world_points.at<
float>(ind * 3 + 1) = (
float)f.p3d.y;
687 world_points.at<
float>(ind * 3 + 2) = (
float)f.p3d.z;
688 image_observations.at<
float>(ind * 2 + 0) = (
float)f.p2d.x;
689 image_observations.at<
float>(ind * 2 + 1) = (
float)f.p2d.y;
691 w.at<
float>(ind * 2 + 1) = w.at<
float>(ind * 2 + 0) = (*weights)[iter->first];
703 cv::Mat world_points_sub = world_points(r);
706 cv::Mat image_observations_sub = image_observations(r);
710 cv::Mat w_sub = w(r);
711 ret = UpdatePose(world_points_sub, image_observations_sub, pose, w_sub);
713 cv::Mat w_ones = cv::Mat::ones(w.size(), w.type());
716 image_observations_sub,
720 image_observations.release();
721 world_points.release();
728 template <
typename T>
733 bool needs_has_p2d =
false,
734 bool needs_has_p3d =
false,
735 float average_outlier_limit = 0.f)
738 return Reproject(container, pose, do_handle_test, average_outlier_limit);
741 template <
typename T,
typename F>
746 float average_outlier_limit = 0.f)
748 float reprojection_sum = 0.f;
749 int reprojection_count = 0;
750 float limit_sq = average_outlier_limit * average_outlier_limit;
751 typename std::map<int, T>::iterator iter = container.begin();
752 typename std::map<int, T>::iterator iter_end = container.end();
753 for (; iter != iter_end; iter++) {
755 if (!do_handle_test(f))
761 float dist_sq = (f.p2d.x - f.projected_p2d.x) * (f.p2d.x - f.projected_p2d.x);
762 dist_sq += (f.p2d.y - f.projected_p2d.y) * (f.p2d.y - f.projected_p2d.y);
763 if ((limit_sq == 0) || (dist_sq < limit_sq)) {
764 reprojection_sum += sqrt(dist_sq);
765 reprojection_count++;
768 if (reprojection_count == 0)
770 return reprojection_sum / reprojection_count;
777 template <
typename T>
780 float reprojection_error_limit,
785 Reproject(container, pose, type_id,
false,
false);
786 DoEraseTest<T> do_erase_test(reprojection_error_limit,
false,
false, type_id);
795 UpdateRotation(
const cv::Mat &object_points, cv::Mat &image_points, cv::Mat &rot, cv::Mat &tra);
799 UpdatePose(
const cv::Mat &object_points, cv::Mat &image_points,
Pose *pose, cv::Mat &weights);
803 cv::Mat & image_points,
811 const cv::Point2f *u1,
812 const cv::Point2f *u2,
827 int id = first_id + marker_id * 4 + corner_id;
838 template <
typename T>
840 PreDetect(std::pair<const int, T> &p,
int type_id)
842 return PreDetect(p.second, type_id);
844 template <
typename T>
846 PreDetect(T &p,
int type_id)
848 if (p.type_id != type_id)
856 GetId(
int marker_id,
int corner_id,
int first_id = 0,
int last_id = 65535)
862 template <
typename T>
866 std::map<int, T> &container,
871 bool visualize =
false,
872 double max_new_marker_error = 0.08,
873 double max_track_error = 0.2,
874 LabelingMethod labeling_method = CVSEQ)
879 typename std::map<int, T>::iterator iter = container.begin();
880 typename std::map<int, T>::iterator iter_end = container.end();
881 for (; iter != iter_end; iter++) {
883 if (f.type_id != type_id)
890 image, cam, track, visualize, max_new_marker_error, max_track_error, labeling_method,
false);
893 for (
size_t i = 0; i < MarkerDetector<M>::markers->size(); i++) {
895 for (
int corner = 0; corner < 4; corner++) {
898 T &f = container[id];
901 f.p2d.x = float(m.marker_corners_img[corner].x);
902 f.p2d.y = float(m.marker_corners_img[corner].y);
910 template <
typename T>
920 cv::Mat m3 = cv::Mat(4, 4, CV_64F);
924 for (
size_t corner = 0; corner < 4; ++corner) {
927 double X_data[4] = {0, 0, 0, 1};
929 X_data[0] = -0.5 * edge_length;
930 X_data[1] = -0.5 * edge_length;
931 }
else if (corner == 1) {
932 X_data[0] = +0.5 * edge_length;
933 X_data[1] = -0.5 * edge_length;
934 }
else if (corner == 2) {
935 X_data[0] = +0.5 * edge_length;
936 X_data[1] = +0.5 * edge_length;
937 }
else if (corner == 3) {
938 X_data[0] = -0.5 * edge_length;
939 X_data[1] = +0.5 * edge_length;
942 cv::Mat X = cv::Mat(4, 1, CV_64F, X_data);
946 T & f = container[id];
948 f.p3d.x = float(X_data[0] / X_data[3]);
949 f.p3d.y = float(X_data[1] / X_data[3]);
950 f.p3d.z = float(X_data[2] / X_data[3]);
962 template <
typename T>
964 MarkersToEC(std::map<int, T> &container,
int type_id = 0,
int first_id = 0,
int last_id = 65535)
967 for (
size_t i = 0; i < marker_indices.size(); i++) {
968 if (marker_status[i] == 0)
970 int marker_id = marker_indices[i];
971 for (
int corner = 0; corner < 4; corner++) {
974 int pc_index = pointcloud_index(marker_id, corner);
975 T & f = container[id];
977 f.p3d.x = float(pointcloud[pc_index].x);
978 f.p3d.y = float(pointcloud[pc_index].y);
979 f.p3d.z = float(pointcloud[pc_index].z);
988 template <
typename T>
990 MarkersFromEC(std::map<int, T> &container,
int type_id = 0,
int first_id = 0,
int last_id = 65535)
996 template <
typename T>
998 Save(std::map<int, T> &container,
1003 int last_id = 65535)
1005 if (!MarkersFromEC(container, type_id, first_id, last_id))
1013 template <
typename T>
1020 int last_id = 65535)
1024 return MarkersToEC(container, type_id, first_id, last_id);
This file implements a camera used for projecting points and computing homographies.
This file implements a generic marker detector.
This file implements a multi-marker.
This file implements a feature tracker.
Version of Camera using external container.
bool UpdatePose(std::map< int, T > &container, Pose *pose, int type_id=-1, std::map< int, double > *weights=0)
Update the pose using the items with matching type_id.
bool CalcExteriorOrientation(std::map< int, T > &container, Pose *pose, F do_handle_test)
Calculate the pose using the items matching the given functor.
void Get3dOnDepth(const Pose *pose, cv::Point2f p2d, float depth, cv::Point3f &p3d)
Get 3D-coordinate for 2D feature assuming certain depth.
bool UpdateRotation(const cv::Mat &object_points, cv::Mat &image_points, Pose *pose)
Update pose rotations based on new observations.
void Undistort(std::map< int, T > &container, F &do_handle_test)
Undistort the items matching the given functor.
bool UpdatePose(const cv::Mat &object_points, cv::Mat &image_points, Pose *pose, cv::Mat &weights)
Update existing pose based on new observations.
void Undistort(std::map< int, T > &container, int type_id=-1)
Undistort the items with matching type_id.
void Get3dOnPlane(const Pose *pose, cv::Point2f p2d, cv::Point3f &p3d)
Get 3D-coordinate for 2D feature on the plane defined by the pose (z == 0)
bool UpdateRotation(std::map< int, T > &container, Pose *pose, int type_id=-1)
Update the rotation in pose using the items with matching type_id.
bool UpdatePose(std::map< int, T > &container, Pose *pose, F do_handle_test, std::map< int, double > *weights=0)
Update the pose using the items matching the given functor.
bool CalcExteriorOrientation(std::map< int, T > &container, Pose *pose, int type_id=-1)
Calculate the pose using the items with matching type_id.
int EraseUsingReprojectionError(std::map< int, T > &container, float reprojection_error_limit, int type_id=-1, Pose *pose=0)
Erases the items matching with type_id having large reprojection error. If type_id == -1 doesn't test...
bool UpdateRotation(const cv::Mat &object_points, cv::Mat &image_points, cv::Mat &rot, cv::Mat &tra)
Update pose rotations (in rodriques&tra) based on new observations.
void Distort(std::map< int, T > &container, F &do_handle_test)
Distort the items matching the given functor.
float Reproject(std::map< int, T > &container, Pose *pose, F do_handle_test, float average_outlier_limit=0.f)
Projects p3d in the items matching with the given functor.
bool UpdatePose(const cv::Mat &object_points, cv::Mat &image_points, cv::Mat &rodriques, cv::Mat &tra, cv::Mat &weights)
Update existing pose (in rodriques&tra) based on new observations.
bool UpdateRotation(std::map< int, T > &container, Pose *pose, F do_handle_test)
Update the rotation in pose using the items matching the given functor.
float Reproject(std::map< int, T > &container, Pose *pose, int type_id=-1, bool needs_has_p2d=false, bool needs_has_p3d=false, float average_outlier_limit=0.f)
Projects p3d in the items matching with type_id into projected_p2d . If type_id == -1 doesn't test th...
void Distort(std::map< int, T > &container, int type_id=-1)
Distort the items with matching type_id.
bool ReconstructFeature(const Pose *pose1, const Pose *pose2, const cv::Point2f *u1, const cv::Point2f *u2, cv::Point3f *p3d, double limit)
Reconstruct 3D point using two camera poses and corresponding undistorted image feature positions.
Simple Camera class for calculating distortions, orientation or projections with pre-calibrated camer...
void ProjectPoint(const cv::Point3d &pw, const Pose *pose, cv::Point2d &pi) const
Project one point.
void CalcExteriorOrientation(const std::vector< cv::Point3d > &pw, const std::vector< PointDouble > &pi, cv::Mat &rodriques, cv::Mat &tra) const
Calculate exterior orientation.
This is default functor for testing which items in the container should be erased.
This is a default functor for testing which items in the container should be handled by each method.
Basic structure to be usable with EC methods.
Version of MarkerDetector using external container.
int Detect(cv::Mat &image, Camera *cam, std::map< int, T > &container, int type_id=0, int first_id=0, int last_id=65535, bool track=false, bool visualize=false, double max_new_marker_error=0.08, double max_track_error=0.2, LabelingMethod labeling_method=CVSEQ)
Detect markers in the image and fill in their 2D-positions in the given external container.
void MarkerToEC(std::map< int, T > &container, int marker_id, double edge_length, Pose &pose, int type_id=0, int first_id=0, int last_id=65535)
Fill in 3D-coordinates for marker_id marker corners using edge_length and pose.
MarkerDetector for detecting markers of type M
int Detect(cv::Mat &image, Camera *cam, bool track=false, bool visualize=false, double max_new_marker_error=0.08, double max_track_error=0.2, LabelingMethod labeling_method=CVSEQ, bool update_pose=true)
Detect Marker 's from image
Version of MultiMarker using external container.
bool MarkersToEC(std::map< int, T > &container, int type_id=0, int first_id=0, int last_id=65535)
Fill in 3D-coordinates for marker corners to the given container.
bool Load(std::map< int, T > &container, const char *fname, FILE_FORMAT format=FILE_FORMAT_DEFAULT, int type_id=0, int first_id=0, int last_id=65535)
Fill in 3D-coordinates for marker corners to the given container using save MultiMarker file.
Base class for using MultiMarker.
bool Save(const char *fname, FILE_FORMAT format=FILE_FORMAT_DEFAULT)
Saves multi marker configuration to a text file.
bool Load(const char *fname, FILE_FORMAT format=FILE_FORMAT_DEFAULT)
Loads multi marker configuration from a text file.
Pose representation derived from the Rotation class
void GetMatrix(cv::Mat &mat) const
Version of TrackerFeatures using external container.
bool AddFeatures(std::map< int, T > &container, int type_id=0, int first_id=0, int last_id=65535)
add features to the previously tracked frame if there are less than min_features
bool Track(cv::Mat &img, cv::Mat &mask, std::map< int, T > &container, int type_id=-1, int first_id=-1, int last_id=-1)
Track features with matching type id. New features will have id's in the specified id range.
TrackerFeaturesEC(int _max_features=100, int _min_features=90, double _quality_level=0.01, double _min_distance=10, int _pyr_levels=4, int win_size=6)
Constructor.
void Purge()
Purge features that are considerably closer than the defined min_distance.
int EraseNonTracked(std::map< int, T > &container, int type_id=-1)
Erases the items matching with type_id having has_p2d == false . If type_id == -1 doesn't test the ty...
bool Track(cv::Mat &img, cv::Mat &mask, std::map< int, T > &container, F do_handle_test, int type_id=0, int first_id=-1, int last_id=-1)
Track features matching the given functor. New features will have id's in the specified id range.
TrackerFeatures tracks features using OpenCV's cvGoodFeaturesToTrack and cvCalcOpticalFlowPyrLK
int Purge()
Purge features that are considerably closer than the defined min_distance.
int * ids
Track result: ID:s for current features
int feature_count
Track result: count of current features
int AddFeatures(cv::Mat &mask)
add features to the previously tracked frame if there are less than min_features
double TrackHid(const cv::Mat &img, cv::Mat &mask, bool add_features=true)
Reset track features on specified mask area.
std::vector< cv::Point2f > features
Track result: current features
@ FILE_FORMAT_DEFAULT
Default file format.
int MarkerIdToContainerId(int marker_id, int corner_id, int first_id=0, int last_id=65535)
Calculate the index used in external container map for specified marker_id.
int EraseItemsEC(std::map< int, T > &container, F do_erase_test)
Erasing items from container using DoEraseTest type functor.