From fe23f794ab5de6cc14e1307e17d51bd0a20eb061 Mon Sep 17 00:00:00 2001
From: PengGao <peng.gaoc@gmail.com>
Date: Fri, 5 May 2023 13:55:36 +0800
Subject: [PATCH] Merge PR #29 (#42)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* fix(image.cpp): 🐛 incorrect image data copy

* test: 🧪 image data copy unittest

* fix: lint

* chore: 🍻 some code trick

---------

Co-authored-by: Ruitard <gallifrey.zhang@foxmail.com>
---
 cpp/tests/test_image.cpp                |  6 ++++++
 cpp/xrprimer/calibration/calibrator.cpp | 13 +++++++------
 cpp/xrprimer/data_structure/image.cpp   | 11 +++++------
 cpp/xrprimer/data_structure/pose.cpp    |  5 +----
 4 files changed, 19 insertions(+), 16 deletions(-)

diff --git a/cpp/tests/test_image.cpp b/cpp/tests/test_image.cpp
index f00f17c..139560f 100644
--- a/cpp/tests/test_image.cpp
+++ b/cpp/tests/test_image.cpp
@@ -64,12 +64,18 @@ TEST_CASE("Image ops", "Image") {
         REQUIRE(new_val.width() == black.cols);
         REQUIRE(new_val.height() == black.rows);
         REQUIRE(new_val.step() == black.step);
+        REQUIRE(std::memcmp(new_val.data(), black.data,
+                            new_val.width() * new_val.height() *
+                                new_val.elemSize()) == 0);
 
         Image new_val2;
         i_black.copyTo(&new_val2);
         REQUIRE(new_val2.width() == black.cols);
         REQUIRE(new_val2.height() == black.rows);
         REQUIRE(new_val2.step() == black.step);
+        REQUIRE(std::memcmp(new_val2.data(), black.data,
+                            new_val2.width() * new_val2.height() *
+                                new_val2.elemSize()) == 0);
     }
 
     SECTION("image_to_opencv") {
diff --git a/cpp/xrprimer/calibration/calibrator.cpp b/cpp/xrprimer/calibration/calibrator.cpp
index 85ad735..170596c 100644
--- a/cpp/xrprimer/calibration/calibrator.cpp
+++ b/cpp/xrprimer/calibration/calibrator.cpp
@@ -153,8 +153,8 @@ bool MultiCalibrator::Init() {
         std::vector<cv::Point3f> p3ds;
         for (int row = 0; row < pattern_size.height; row++)
             for (int col = 0; col < pattern_size.width; col++)
-                p3ds.emplace_back(cv::Point3f(col * square_size.width,
-                                              row * square_size.height, 0.f));
+                p3ds.emplace_back(col * square_size.width,
+                                  row * square_size.height, 0.f);
 
         int camId = 0;
         // camera/points
@@ -349,7 +349,9 @@ struct ReprojCostFunctor {
 void MultiCalibrator::OptimizeExtrinsics() {
     printf("Start bundle.\n");
     std::vector<Eigen::Vector3d> rs;
+    rs.reserve(pinhole_params.size());
     std::vector<Eigen::Vector3d> ts;
+    ts.reserve(pinhole_params.size());
     for (const auto &cam : pinhole_params) {
         Eigen::AngleAxisf angleAxis(cam.extrinsic_r_);
         rs.push_back(Eigen::Vector3f(angleAxis.axis() * angleAxis.angle())
@@ -366,10 +368,9 @@ void MultiCalibrator::OptimizeExtrinsics() {
 
     ceres::Problem problem;
     for (int img_idx = 0; img_idx < point2d_lists.size(); img_idx++) {
-        std::vector<std::vector<Eigen::Vector2f>> point2ds;
-        std::vector<std::vector<MathUtil::Matrix34f>> projs;
-        point2ds.resize(point_count);
-        projs.resize(point_count);
+        std::vector<std::vector<Eigen::Vector2f>> point2ds(point_count);
+        std::vector<std::vector<MathUtil::Matrix34f>> projs(point_count);
+
         // triangulate
         int cam_idx = 0;
         for (const auto &iter : point2d_lists[img_idx]) {
diff --git a/cpp/xrprimer/data_structure/image.cpp b/cpp/xrprimer/data_structure/image.cpp
index 08099e5..9d18cb8 100644
--- a/cpp/xrprimer/data_structure/image.cpp
+++ b/cpp/xrprimer/data_structure/image.cpp
@@ -1,12 +1,12 @@
 // Copyright (c) OpenXRLab. All rights reserved.
 
 #include <algorithm>
+#include <cstdint>
+#include <cstdio>
+#include <cstring>
 #include <data_structure/image.h>
 #include <map>
 #include <memory>
-#include <stdint.h>
-#include <stdio.h>
-#include <string.h>
 #include <vector>
 
 /**
@@ -93,9 +93,8 @@ class Image::Impl {
 
         if (impl->format_ == format_ && impl->width_ == width_ &&
             impl->height_ == height_) {
-            for (int h = 0; h < height_; ++h) {
-                memcpy(impl->data_, this->data_, impl->width_);
-            }
+
+            std::memcpy(impl->data_, this->data_, this->height_ * this->step_);
 
         } else {
             // TODO: mismatch format, maybe data is external
diff --git a/cpp/xrprimer/data_structure/pose.cpp b/cpp/xrprimer/data_structure/pose.cpp
index 6f84538..55c45c1 100644
--- a/cpp/xrprimer/data_structure/pose.cpp
+++ b/cpp/xrprimer/data_structure/pose.cpp
@@ -86,10 +86,7 @@ Pose Pose::Scale(double s) const {
 }
 
 Eigen::Vector3d Pose::Center() const {
-    if (this) {
-        return -(this->quaternion_.inverse() * this->position_);
-    }
-    return Eigen::Vector3d::Zero();
+    return -(this->quaternion_.inverse() * this->position_);
 }
 
 void Pose::PoseMult(const Pose &lhs, const Pose &rhs) {