32 #define TEST_NAME "base/reconstruction"
46 const size_t kNumPoints2D = 10;
53 for (
image_t image_id = 1; image_id <= num_images; ++image_id) {
55 image.SetImageId(image_id);
59 std::vector<Eigen::Vector2d>(kNumPoints2D, Eigen::Vector2d::Zero()));
62 correspondence_graph->
AddImage(image_id, kNumPoints2D);
65 reconstruction->
SetUp(correspondence_graph);
70 BOOST_CHECK_EQUAL(reconstruction.
NumCameras(), 0);
71 BOOST_CHECK_EQUAL(reconstruction.
NumImages(), 0);
86 BOOST_CHECK_EQUAL(reconstruction.
Cameras().count(camera.
CameraId()), 1);
87 BOOST_CHECK_EQUAL(reconstruction.
Cameras().size(), 1);
88 BOOST_CHECK_EQUAL(reconstruction.
NumCameras(), 1);
89 BOOST_CHECK_EQUAL(reconstruction.
NumImages(), 0);
101 BOOST_CHECK_EQUAL(reconstruction.
Image(1).
ImageId(), 1);
103 BOOST_CHECK_EQUAL(reconstruction.
Images().count(1), 1);
104 BOOST_CHECK_EQUAL(reconstruction.
Images().size(), 1);
105 BOOST_CHECK_EQUAL(reconstruction.
NumCameras(), 0);
106 BOOST_CHECK_EQUAL(reconstruction.
NumImages(), 1);
108 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 0);
118 BOOST_CHECK_EQUAL(reconstruction.
Points3D().count(point3D_id), 1);
119 BOOST_CHECK_EQUAL(reconstruction.
Points3D().size(), 1);
120 BOOST_CHECK_EQUAL(reconstruction.
NumCameras(), 0);
121 BOOST_CHECK_EQUAL(reconstruction.
NumImages(), 0);
123 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
125 BOOST_CHECK_EQUAL(reconstruction.
Point3DIds().count(point3D_id), 1);
135 reconstruction.
AddPoint3D(Eigen::Vector3d::Random(), track);
156 Eigen::Matrix<uint8_t, 3, 1>(0, 0, 0);
162 Eigen::Matrix<uint8_t, 3, 1>(20, 20, 20);
167 BOOST_CHECK(reconstruction.
ExistsPoint3D(merged_point3D_id));
176 BOOST_CHECK(reconstruction.
Point3D(merged_point3D_id)
178 .isApprox(Eigen::Vector3d(0.5, 0.5, 0.5)));
179 BOOST_CHECK_EQUAL(reconstruction.
Point3D(merged_point3D_id).
Color(),
241 BOOST_CHECK_LT(std::abs(reconstruction.
Image(1).
Tvec(2) + 10), 1
e-6);
242 BOOST_CHECK_LT(std::abs(reconstruction.
Image(2).
Tvec(2)), 1
e-6);
243 BOOST_CHECK_LT(std::abs(reconstruction.
Image(3).
Tvec(2) - 10), 1
e-6);
248 BOOST_CHECK_LT(std::abs(reconstruction.
Image(1).
Tvec(2) + 5), 1
e-6);
249 BOOST_CHECK_LT(std::abs(reconstruction.
Image(2).
Tvec(2)), 1
e-6);
250 BOOST_CHECK_LT(std::abs(reconstruction.
Image(3).
Tvec(2) - 5), 1
e-6);
252 BOOST_CHECK_LT(std::abs(reconstruction.
Image(1).
Tvec(2) + 2.5), 1
e-6);
253 BOOST_CHECK_LT(std::abs(reconstruction.
Image(2).
Tvec(2)), 1
e-6);
254 BOOST_CHECK_LT(std::abs(reconstruction.
Image(3).
Tvec(2) - 2.5), 1
e-6);
256 BOOST_CHECK_LT(std::abs(reconstruction.
Image(1).
Tvec(2) + 5), 1
e-6);
257 BOOST_CHECK_LT(std::abs(reconstruction.
Image(2).
Tvec(2)), 1
e-6);
258 BOOST_CHECK_LT(std::abs(reconstruction.
Image(3).
Tvec(2) - 5), 1
e-6);
279 BOOST_CHECK_LT(std::abs(reconstruction.
Image(1).
Tvec(2) + 5), 1
e-6);
280 BOOST_CHECK_LT(std::abs(reconstruction.
Image(2).
Tvec(2)), 1
e-6);
281 BOOST_CHECK_LT(std::abs(reconstruction.
Image(3).
Tvec(2) - 5), 1
e-6);
282 BOOST_CHECK_LT(std::abs(reconstruction.
Image(4).
Tvec(2) + 3.75), 1
e-6);
283 BOOST_CHECK_LT(std::abs(reconstruction.
Image(5).
Tvec(2) + 2.5), 1
e-6);
284 BOOST_CHECK_LT(std::abs(reconstruction.
Image(6).
Tvec(2) - 2.5), 1
e-6);
285 BOOST_CHECK_LT(std::abs(reconstruction.
Image(7).
Tvec(2) - 3.75), 1
e-6);
294 BOOST_CHECK_LT(std::abs(centroid(0)), 1
e-6);
295 BOOST_CHECK_LT(std::abs(centroid(1)), 1
e-6);
296 BOOST_CHECK_LT(std::abs(centroid(2)), 1
e-6);
297 BOOST_CHECK_LT(std::abs(bbox.first(0)), 1
e-6);
298 BOOST_CHECK_LT(std::abs(bbox.first(1)), 1
e-6);
299 BOOST_CHECK_LT(std::abs(bbox.first(2)), 1
e-6);
300 BOOST_CHECK_LT(std::abs(bbox.second(0)), 1
e-6);
301 BOOST_CHECK_LT(std::abs(bbox.second(1)), 1
e-6);
302 BOOST_CHECK_LT(std::abs(bbox.second(2)), 1
e-6);
310 BOOST_CHECK_LT(std::abs(centroid(0) - 1.0), 1
e-6);
311 BOOST_CHECK_LT(std::abs(centroid(1) - 1.0), 1
e-6);
312 BOOST_CHECK_LT(std::abs(centroid(2) - 1.0), 1
e-6);
313 BOOST_CHECK_LT(std::abs(bbox.first(0)), 1
e-6);
314 BOOST_CHECK_LT(std::abs(bbox.first(1)), 1
e-6);
315 BOOST_CHECK_LT(std::abs(bbox.first(2)), 1
e-6);
316 BOOST_CHECK_LT(std::abs(bbox.second(0) - 3.0), 1
e-6);
317 BOOST_CHECK_LT(std::abs(bbox.second(1) - 3.0), 1
e-6);
318 BOOST_CHECK_LT(std::abs(bbox.second(2) - 3.0), 1
e-6);
328 point_id = reconstruction.
AddPoint3D(Eigen::Vector3d(0.5, 0.5, 0.0),
Track());
330 point_id = reconstruction.
AddPoint3D(Eigen::Vector3d(1.0, 1.0, 0.0),
Track());
332 point_id = reconstruction.
AddPoint3D(Eigen::Vector3d(0.0, 0.0, 0.5),
Track());
334 point_id = reconstruction.
AddPoint3D(Eigen::Vector3d(0.5, 0.5, 1.0),
Track());
338 BOOST_CHECK_EQUAL(reconstruction.
NumCameras(), 1);
339 BOOST_CHECK_EQUAL(reconstruction.
NumImages(), 3);
341 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 5);
343 std::pair<Eigen::Vector3d, Eigen::Vector3d> bbox;
346 bbox.first = Eigen::Vector3d(-1, -1, -1);
347 bbox.second = Eigen::Vector3d(-0.5, -0.5, -0.5);
350 BOOST_CHECK_EQUAL(recon1.
NumImages(), 3);
355 bbox.first = Eigen::Vector3d(0.0, 0.0, 0.0);
356 bbox.second = Eigen::Vector3d(0.75, 0.75, 0.75);
359 BOOST_CHECK_EQUAL(recon2.
NumImages(), 3);
376 Eigen::Vector3d(0, 1, 2)));
378 Eigen::Vector3d(0, 1, 2));
379 BOOST_CHECK_EQUAL(reconstruction.
Point3D(point3D_id).
XYZ(),
380 Eigen::Vector3d(2, 3, 4));
388 &reconstruction.
Image(1));
390 &reconstruction.
Image(2));
400 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
401 reconstruction.
FilterPoints3D(0.0, 0.0, std::unordered_set<point3D_t>{});
402 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
404 std::unordered_set<point3D_t>{point3D_id1 + 1});
405 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
407 std::unordered_set<point3D_t>{point3D_id1});
408 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 0);
413 std::unordered_set<point3D_t>{point3D_id2});
414 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 0);
420 std::unordered_set<point3D_t>{point3D_id3});
421 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
423 std::unordered_set<point3D_t>{point3D_id3});
424 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 0);
430 std::unordered_set<point3D_t>{point3D_id4});
431 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
433 std::unordered_set<point3D_t>{point3D_id4});
434 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 0);
443 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
445 std::unordered_set<image_t>{});
446 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
448 std::unordered_set<image_t>{1});
449 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
452 std::unordered_set<image_t>{2});
453 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
455 std::unordered_set<image_t>{1});
456 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 0);
462 std::unordered_set<image_t>{1});
463 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
465 std::unordered_set<image_t>{1});
466 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 0);
472 std::unordered_set<image_t>{1});
473 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
475 std::unordered_set<image_t>{1});
476 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 0);
484 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
486 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 0);
491 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 0);
497 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
499 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 0);
505 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
507 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 0);
516 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
518 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
519 reconstruction.
Point3D(point3D_id1).
XYZ(2) = 0.001;
521 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
522 reconstruction.
Point3D(point3D_id1).
XYZ(2) = 0.0;
524 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
526 reconstruction.
Point3D(point3D_id1).
XYZ(2) = 0.001;
528 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 1);
529 reconstruction.
Point3D(point3D_id1).
XYZ(2) = 0.0;
531 BOOST_CHECK_EQUAL(reconstruction.
NumPoints3D(), 0);
std::shared_ptr< core::Tensor > image
void InitializeWithName(const std::string &model_name, const double focal_length, const size_t width, const size_t height)
void InitializeWithId(const int model_id, const double focal_length, const size_t width, const size_t height)
camera_t CameraId() const
void SetCameraId(const camera_t camera_id)
void AddImage(const image_t image_id, const size_t num_points2D)
point2D_t NumPoints3D() const
const Eigen::Vector3d & Tvec() const
const class Point2D & Point2D(const point2D_t point2D_idx) const
bool IsRegistered() const
Eigen::Vector3d ProjectionCenter() const
point3D_t Point3DId() const
void SetError(const double error)
const Eigen::Vector3d & XYZ() const
const class Track & Track() const
const Eigen::Vector3ub & Color() const
size_t FilterAllPoints3D(const double max_reproj_error, const double min_tri_angle)
Reconstruction Crop(const std::pair< Eigen::Vector3d, Eigen::Vector3d > &bbox) const
double ComputeMeanTrackLength() const
point3D_t MergePoints3D(const point3D_t point3D_id1, const point3D_t point3D_id2)
size_t NumRegImages() const
void Transform(const SimilarityTransform3 &tform)
std::pair< Eigen::Vector3d, Eigen::Vector3d > ComputeBoundingBox(const double p0=0.0, const double p1=1.0) const
size_t FilterObservationsWithNegativeDepth()
void Normalize(const double extent=10.0, const double p0=0.1, const double p1=0.9, const bool use_images=true)
bool IsImageRegistered(const image_t image_id) const
void AddImage(const class Image &image)
const std::unordered_map< image_t, class Image > & Images() const
void AddCamera(const class Camera &camera)
std::vector< image_t > FilterImages(const double min_focal_length_ratio, const double max_focal_length_ratio, const double max_extra_param)
void DeleteObservation(const image_t image_id, const point2D_t point2D_idx)
size_t ComputeNumObservations() const
Eigen::Vector3d ComputeCentroid(const double p0=0.1, const double p1=0.9) const
const class Image & Image(const image_t image_id) const
std::unordered_set< point3D_t > Point3DIds() const
size_t NumPoints3D() const
const class Camera & Camera(const camera_t camera_id) const
const std::unordered_map< camera_t, class Camera > & Cameras() const
const std::unordered_map< point3D_t, class Point3D > & Points3D() const
bool ExistsCamera(const camera_t camera_id) const
void AddObservation(const point3D_t point3D_id, const TrackElement &track_el)
const class Image * FindImageWithName(const std::string &name) const
void DeRegisterImage(const image_t image_id)
size_t FilterPoints3D(const double max_reproj_error, const double min_tri_angle, const std::unordered_set< point3D_t > &point3D_ids)
size_t NumImagePairs() const
void DeletePoint3D(const point3D_t point3D_id)
double ComputeMeanReprojectionError() const
bool ExistsImage(const image_t image_id) const
double ComputeMeanObservationsPerRegImage() const
void RegisterImage(const image_t image_id)
size_t FilterPoints3DInImages(const double max_reproj_error, const double min_tri_angle, const std::unordered_set< image_t > &image_ids)
size_t NumCameras() const
const class Point3D & Point3D(const point3D_t point3D_id) const
point3D_t AddPoint3D(const Eigen::Vector3d &xyz, const Track &track, const Eigen::Vector3ub &color=Eigen::Vector3ub::Zero())
void SetUp(const CorrespondenceGraph *correspondence_graph)
bool ExistsPoint3D(const point3D_t point3D_id) const
void AddElement(const TrackElement &element)
Matrix< uint8_t, 3, 1 > Vector3ub
Eigen::Vector4d ComposeIdentityQuaternion()
std::string to_string(const T &n)
BOOST_AUTO_TEST_CASE(TestEmpty)
void GenerateReconstruction(const image_t num_images, Reconstruction *reconstruction, CorrespondenceGraph *correspondence_graph)
static const int model_id