feat: add ibvs demo test

README.md

@@ -36,6 +36,9 @@ sudo apt-get install libglfw3-dev
 # Assimp
 sudo apt-get install libassimp-dev
 
+# visp
+sudo apt-get install -y libx11-dev liblapack-dev  libv4l-dev libzbar-dev libpthread-stubs0-dev libdc1394-dev nlohmann-json3-dev
+
 [//]: # ()
 [//]: # (# json)
 

cmake/FindExternalLib.cmake

@@ -93,17 +93,15 @@ function(setup_external_libs ARCH)
             endif()
         endif()
 
-        # ---- bin  (protoc / grpc_cpp_plugin) ----
+
+        # ---- bin: if dependency provides tools, install them to <prefix>/bin ----
         if(EXISTS "${FULL_PATH}/bin")
-            # 精确收集你需要的两个
+            file(GLOB _BIN_FILES
-            if(EXISTS "${FULL_PATH}/bin/grpc_cpp_plugin")
+                    LIST_DIRECTORIES false
-                list(APPEND INSTALL_BIN_FILES "${FULL_PATH}/bin/grpc_cpp_plugin")
+                    "${FULL_PATH}/bin/*"
-            endif()
+            )
-            if(EXISTS "${FULL_PATH}/bin/protoc")
+            if(_BIN_FILES)
-                list(APPEND INSTALL_BIN_FILES "${FULL_PATH}/bin/protoc")
+                list(APPEND INSTALL_BIN_FILES ${_BIN_FILES})
-            endif()
-            if(EXISTS "${FULL_PATH}/bin/protoc-31.1.0")
-                list(APPEND INSTALL_BIN_FILES "${FULL_PATH}/bin/protoc-31.1.0")
             endif()
         endif()
 
@@ -163,6 +161,7 @@ function(setup_external_libs ARCH)
         list(REMOVE_DUPLICATES INSTALL_BIN_FILES)
         install(PROGRAMS ${INSTALL_BIN_FILES} DESTINATION bin)
 
+        # After installing, patch RPATH of installed tools to $ORIGIN/../lib
         # After installing, patch RPATH of installed tools to $ORIGIN/../lib
         install(CODE [[
           execute_process(COMMAND bash -lc
@@ -171,15 +170,19 @@ function(setup_external_libs ARCH)
                echo '[install] patchelf not found, skip patch bin rpath'
                exit 0
              fi
+
              echo '[install] patch rpath under: ${CMAKE_INSTALL_PREFIX}/bin'
-             for f in '${CMAKE_INSTALL_PREFIX}/bin/protoc' '${CMAKE_INSTALL_PREFIX}/bin/grpc_cpp_plugin'; do
+             # -perm -111: any executable bit set
-               if [ -f \"$f\" ]; then
+             find '${CMAKE_INSTALL_PREFIX}/bin' -maxdepth 1 -type f -perm -111 | while read -r f; do
-                 patchelf --set-rpath '$ORIGIN/../lib' \"$f\"
+               # only patch ELF binaries (skip scripts/text)
+               if file -b \"$f\" 2>/dev/null | grep -qi 'ELF'; then
+                 patchelf --set-rpath '$ORIGIN/../lib' \"$f\" || true
                fi
              done
              echo '[install] done (bin rpath)'"
           )
         ]])
+
     endif()
 
     if(LIB_COUNT GREATER 0)

cmvr-es/controller/CMakeLists.txt

@@ -1,5 +1,16 @@
+
+
+#  如果报 relocation ... can not be used when making a shared object; recompile with -fPIC  ，说明SRC 中包含了test文件 ，test
+#  中链接 -lgtest -lgtest_main   ，这是静态库,导致 libcontroller.so 被迫依赖 gtest。
+#  所以 add_library(controller SHARED
+#          src/controller_creator.cpp
+#          src/pid_controller.cpp
+#           ❌ 不要把 src/controller_test.cpp 放进来
+#)  其他动态库类似
 file(GLOB SRC
-        ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/src/controller_creator.cpp
+        ${CMAKE_CURRENT_SOURCE_DIR}/src/pid_controller.cpp
+
 )
 
 add_library(controller SHARED ${SRC})
@@ -21,3 +32,30 @@ target_link_libraries(controller PUBLIC
 )
 
 add_library(cmvr_es::controller ALIAS controller)
+
+
+
+
+# --------------------------------------------------------
+# Unit test
+# --------------------------------------------------------
+find_package(VISP REQUIRED)
+find_package(realsense2 REQUIRED)
+add_executable(controller_test
+        ${CMAKE_CURRENT_SOURCE_DIR}/src/controller_test.cpp
+)
+
+target_link_libraries(controller_test
+        PRIVATE
+        cmvr_es::utils
+        cmvr_es::ik_solver
+        cmvr_es::planner
+        cmvr_es::proto
+        cmvr_es::mujoco_viewer
+        gtest
+        gtest_main
+        pthread
+        glog
+        ${VISP_LIBRARIES}
+        realsense2::realsense2
+)

cmvr-es/controller/src/controller_test.cpp

@@ -0,0 +1,867 @@
+//
+// Created by lgv on 2026/2/10.
+//  TEST(contrller_test, visp_test){
+#include "gtest/gtest.h"
+
+#include <algorithm>
+#include <array>
+#include <atomic>
+#include <chrono>
+#include <cmath>
+#include <mutex>
+#include <thread>
+#include <vector>
+
+#include <Eigen/Dense>
+
+#include "ik_solver/include/lawba_ik_solver.h"
+#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
+using namespace cmvr;
+
+
+
+
+#include "gtest/gtest.h"
+
+#include <array>
+#include <cmath>
+#include <cstdio>
+#include <vector>
+
+#include <Eigen/Dense>
+
+#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
+
+// ---- ViSP ----
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
+
+// Created by lgv on 2026/2/10.
+
+#include "gtest/gtest.h"
+
+#include <array>
+#include <cmath>
+#include <cstdio>
+#include <vector>
+
+#include <Eigen/Dense>
+
+#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
+
+// ---- ViSP ----
+#include <visp3/core/vpHomogeneousMatrix.h>
+#include <visp3/core/vpPoint.h>
+#include <visp3/visual_features/vpFeaturePoint.h>
+#include <visp3/vs/vpServo.h>
+
+using namespace cmvr;
+
+namespace {
+
+// clamp helper
+static inline double clamp(double x, double lo, double hi) {
+  return std::max(lo, std::min(hi, x));
+}
+
+// MuJoCo xmat(9) -> Eigen::Matrix3d (row-major)
+static inline Eigen::Matrix3d xmat_to_R(const mjtNum* xmat9) {
+  Eigen::Matrix3d R;
+  R << xmat9[0], xmat9[1], xmat9[2],
+       xmat9[3], xmat9[4], xmat9[5],
+       xmat9[6], xmat9[7], xmat9[8];
+  return R;
+}
+
+// Build vpHomogeneousMatrix from Eigen R,t
+static inline vpHomogeneousMatrix make_cMo_from_Eigen(const Eigen::Matrix3d& R,
+                                                      const Eigen::Vector3d& t) {
+  vpRotationMatrix vR;
+  for (int r = 0; r < 3; ++r)
+    for (int c = 0; c < 3; ++c)
+      vR[r][c] = R(r, c);
+
+  vpTranslationVector vt(t(0), t(1), t(2));
+  vpHomogeneousMatrix cMo(vt, vR);
+  return cMo;
+}
+
+} // namespace
+
+class IBVSHomingViewer : public MuJocoViewer {
+public:
+  using MuJocoViewer::MuJocoViewer;
+
+protected:
+  enum class Mode { HOMING, IBVS };
+
+  void initOnce(mjModel* m, mjData* d) override {
+    // 1) 主视角自由相机 + PiP显示 hand_cam
+    setupCamera(3.0, -170.0, -40.0);
+    enablePiPCamera("hand_cam", 405, 1200, 320, 240);
+
+    // 2) 右臂 7DOF actuator/joint
+    const char* act_names[7] = {
+      "R_SHOULDER_P_pos",
+      "R_SHOULDER_R_pos",
+      "R_SHOULDER_Y_pos",
+      "R_ELBOW_R_pos",
+      "R_WRIST_P_pos",
+      "R_WRIST_Y_pos",
+      "R_WRIST_R_pos"
+    };
+    const char* jnt_names[7] = {
+      "R_SHOULDER_P",
+      "R_SHOULDER_R",
+      "R_SHOULDER_Y",
+      "R_ELBOW_R",
+      "R_WRIST_P",
+      "R_WRIST_Y",
+      "R_WRIST_R"
+    };
+
+    for (int i = 0; i < 7; ++i) {
+      act_ids_[i] = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]);
+      jnt_ids_[i] = mj_name2id(m, mjOBJ_JOINT,    jnt_names[i]);
+
+      if (act_ids_[i] < 0) std::fprintf(stderr, "Cannot find actuator %s\n", act_names[i]);
+      if (jnt_ids_[i] < 0) std::fprintf(stderr, "Cannot find joint %s\n", jnt_names[i]);
+
+      if (jnt_ids_[i] >= 0) {
+        qpos_adr_[i] = m->jnt_qposadr[jnt_ids_[i]];
+        dof_adr_[i]  = m->jnt_dofadr[jnt_ids_[i]];
+      }
+    }
+
+    // 3) 找相机 site + tag
+    cam_site_id_ = mj_name2id(m, mjOBJ_SITE, "R_CAM_SITE");
+    tag_body_id_ = mj_name2id(m, mjOBJ_BODY, "tag_board");
+
+    std::printf("[IBVS] R_CAM_SITE id=%d, tag_board id=%d\n",
+                cam_site_id_, tag_body_id_);
+
+    if (cam_site_id_ < 0 || tag_body_id_ < 0) {
+      std::fprintf(stderr, "[IBVS] Missing R_CAM_SITE or tag_board. Check XML names.\n");
+      ready_ = false;
+      return;
+    }
+
+    // 4) 初始姿态（保证 PiP 里能看到 tag）
+    q_home_ = { 0.25, 1.00, M_PI/2 - 0.2, M_PI/2 - 0.2, -M_PI + 0.5, 0.0, 0.0 };
+
+    // 5) 坐标轴对齐：site相机系 -> ViSP相机系
+    // 你现在截图表现是“画面左上”，但 t_co.x,y 是正，说明 x/y 反了
+    // 先试这个：翻转 x,y
+    R_cv_ = (Eigen::Matrix3d() <<
+      -1, 0, 0,
+       0,-1, 0,
+       0, 0, 1).finished();
+
+    // 6) ViSP IBVS 参数
+    tag_half_ = 0.06;   // 12cm tag -> half 6cm
+    Z_des_    = 0.34;
+    lambda_   = 0.7;
+
+    task_.setServo(vpServo::EYEINHAND_CAMERA);
+    task_.setInteractionMatrixType(vpServo::CURRENT);
+    task_.setLambda(lambda_);
+
+    obj_pts_[0].setWorldCoordinates(-tag_half_, -tag_half_, 0.0);
+    obj_pts_[1].setWorldCoordinates( tag_half_, -tag_half_, 0.0);
+    obj_pts_[2].setWorldCoordinates( tag_half_,  tag_half_, 0.0);
+    obj_pts_[3].setWorldCoordinates(-tag_half_,  tag_half_, 0.0);
+
+    // desired: 正对 + 距离 Z_des
+    {
+      vpTranslationVector t_des(0.0, 0.0, Z_des_);
+      vpRotationMatrix R_des; R_des.eye();
+      vpHomogeneousMatrix cMo_des(t_des, R_des);
+
+      for (int i = 0; i < 4; ++i) {
+        obj_pts_[i].track(cMo_des);
+        s_star_[i].buildFrom(obj_pts_[i].get_x(),
+                             obj_pts_[i].get_y(),
+                             obj_pts_[i].get_Z());
+
+        s_cur_[i].buildFrom(0.0, 0.0, 1.0);
+        task_.addFeature(s_cur_[i], s_star_[i]);
+      }
+    }
+
+    // 初始化 q_cmd 为当前 qpos，避免突变
+    for (int i = 0; i < 7; ++i) {
+      q_cmd_[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
+    }
+
+    mode_ = Mode::HOMING;
+    home_hold_acc_ = 0.0;
+    step_count_ = 0;
+    ready_ = true;
+  }
+
+  void controlCallback(mjModel* m, mjData* d) override {
+    if (!ready_) return;
+    const double dt = m->opt.timestep;
+
+    // ======================
+    // A) HOMING
+    // ======================
+    if (mode_ == Mode::HOMING) {
+      double max_err = 0.0;
+      for (int i = 0; i < 7; ++i) {
+        if (act_ids_[i] < 0 || qpos_adr_[i] < 0) continue;
+        d->ctrl[act_ids_[i]] = q_home_[i];
+        const double qi = d->qpos[qpos_adr_[i]];
+        max_err = std::max(max_err, std::abs(qi - q_home_[i]));
+      }
+
+      if (max_err < home_tol_) home_hold_acc_ += dt;
+      else home_hold_acc_ = 0.0;
+
+      if (home_hold_acc_ > home_hold_time_) {
+        for (int i = 0; i < 7; ++i) {
+          if (qpos_adr_[i] >= 0) q_cmd_[i] = d->qpos[qpos_adr_[i]];
+        }
+        mode_ = Mode::IBVS;
+        std::printf("[IBVS] switch HOMING -> IBVS\n");
+      }
+      return;
+    }
+
+    // ======================
+    // B) IBVS
+    // ======================
+
+    // 1) camera(site) pose in world
+    const mjtNum* pc = d->site_xpos + 3 * cam_site_id_;
+    const mjtNum* Rc9 = d->site_xmat + 9 * cam_site_id_;
+    Eigen::Vector3d p_cw(pc[0], pc[1], pc[2]);
+    Eigen::Matrix3d R_cw = xmat_to_R(Rc9);      // site(cam)->world
+    Eigen::Matrix3d R_wc = R_cw.transpose();    // world->site(cam)
+
+    // 2) tag_board body pose in world
+    const mjtNum* po = d->xpos + 3 * tag_body_id_;
+    const mjtNum* Ro9 = d->xmat + 9 * tag_body_id_;
+    Eigen::Vector3d p_ow(po[0], po[1], po[2]);
+    Eigen::Matrix3d R_ow = xmat_to_R(Ro9);
+
+    // 3) object->camera(site)
+    Eigen::Matrix3d R_co_site = R_wc * R_ow;
+    Eigen::Vector3d t_co_site = R_wc * (p_ow - p_cw);
+
+    // 4) site相机系 -> ViSP相机系（对齐）
+    Eigen::Matrix3d R_co = R_cv_ * R_co_site;
+    Eigen::Vector3d t_co = R_cv_ * t_co_site;
+
+    if ((step_count_ % 60) == 0) {
+      std::printf("[IBVS] t_co = [%.3f %.3f %.3f]\n", t_co.x(), t_co.y(), t_co.z());
+    }
+
+    vpHomogeneousMatrix cMo = make_cMo_from_Eigen(R_co, t_co);
+
+    // 5) current features
+    for (int i = 0; i < 4; ++i) {
+      obj_pts_[i].track(cMo);
+      double x = obj_pts_[i].get_x();
+      double y = obj_pts_[i].get_y();
+      double Z = std::max(obj_pts_[i].get_Z(), 0.05);
+      s_cur_[i].buildFrom(x, y, Z);
+    }
+
+    // 6) ViSP control law -> v_c (ViSP camera frame)
+    vpColVector v_c = task_.computeControlLaw();
+
+    // 限幅（关键）
+    for (int k = 0; k < 6; ++k) {
+      v_c[k] = clamp(v_c[k], -vmax6_[k], vmax6_[k]);
+    }
+
+    if ((step_count_ % 60) == 0) {
+      std::printf("[IBVS] v_c = [%+.3f %+.3f %+.3f %+.3f %+.3f %+.3f]\n",
+                  v_c[0], v_c[1], v_c[2], v_c[3], v_c[4], v_c[5]);
+    }
+
+    // 7) ViSP相机速度 -> site相机速度（逆对齐）
+    Eigen::Vector3d v_visp(v_c[0], v_c[1], v_c[2]);
+    Eigen::Vector3d w_visp(v_c[3], v_c[4], v_c[5]);
+
+    Eigen::Vector3d v_site = R_cv_.transpose() * v_visp;
+    Eigen::Vector3d w_site = R_cv_.transpose() * w_visp;
+
+    // 8) site相机速度 -> world twist（给 world Jacobian 用）
+    Eigen::Vector3d v_w = R_cw * v_site;
+    Eigen::Vector3d w_w = R_cw * w_site;
+
+    Eigen::Matrix<double,6,1> twist_w;
+    twist_w << v_w(0), v_w(1), v_w(2), w_w(0), w_w(1), w_w(2);
+
+    // 9) Jacobian for R_CAM_SITE (world)
+    std::vector<mjtNum> jacp(3 * m->nv);
+    std::vector<mjtNum> jacr(3 * m->nv);
+    mj_jacSite(m, d, jacp.data(), jacr.data(), cam_site_id_);
+
+    Eigen::Matrix<double,6,7> J;
+    J.setZero();
+    for (int j = 0; j < 7; ++j) {
+      const int dof = dof_adr_[j];
+      if (dof < 0) continue;
+
+      J(0,j) = jacp[0*m->nv + dof];
+      J(1,j) = jacp[1*m->nv + dof];
+      J(2,j) = jacp[2*m->nv + dof];
+      J(3,j) = jacr[0*m->nv + dof];
+      J(4,j) = jacr[1*m->nv + dof];
+      J(5,j) = jacr[2*m->nv + dof];
+    }
+
+    // 10) DLS inverse: qdot
+    Eigen::Matrix<double,6,6> A = J * J.transpose();
+    A += (mu_*mu_) * Eigen::Matrix<double,6,6>::Identity();
+    Eigen::Matrix<double,7,1> qdot = J.transpose() * A.inverse() * twist_w;
+
+    for (int i = 0; i < 7; ++i) {
+      qdot(i) = clamp(qdot(i), -qdot_max_, qdot_max_);
+    }
+
+    // 11) integrate -> position targets
+    for (int i = 0; i < 7; ++i) {
+      q_cmd_[i] += qdot(i) * dt;
+
+      if (jnt_ids_[i] >= 0 && m->jnt_limited[jnt_ids_[i]]) {
+        const double lo = m->jnt_range[2*jnt_ids_[i] + 0];
+        const double hi = m->jnt_range[2*jnt_ids_[i] + 1];
+        q_cmd_[i] = clamp(q_cmd_[i], lo, hi);
+      }
+
+      if (act_ids_[i] >= 0) {
+        d->ctrl[act_ids_[i]] = q_cmd_[i];
+      }
+    }
+
+    // 12) sanity: 看看机械臂是否真的在动
+    if ((step_count_ % 120) == 0 && qpos_adr_[0] >= 0 && act_ids_[0] >= 0) {
+      std::printf("[IBVS] qpos0=%.3f ctrl0=%.3f\n",
+                  d->qpos[qpos_adr_[0]], d->ctrl[act_ids_[0]]);
+    }
+
+    ++step_count_;
+  }
+
+  void onReset(mjModel* m, mjData* d) override {
+    (void)m;
+    for (int i = 0; i < 7; ++i) {
+      if (qpos_adr_[i] >= 0) q_cmd_[i] = d->qpos[qpos_adr_[i]];
+    }
+    mode_ = Mode::HOMING;
+    home_hold_acc_ = 0.0;
+    step_count_ = 0;
+  }
+
+private:
+  bool ready_{false};
+
+  std::array<int,7> act_ids_{};
+  std::array<int,7> jnt_ids_{};
+  std::array<int,7> qpos_adr_{ { -1,-1,-1,-1,-1,-1,-1 } };
+  std::array<int,7> dof_adr_{  { -1,-1,-1,-1,-1,-1,-1 } };
+
+  int cam_site_id_{-1};
+  int tag_body_id_{-1};
+
+  // frame align
+  Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()};
+
+  // homing
+  Mode mode_{Mode::HOMING};
+  std::array<double,7> q_home_{ {0,0,0,0,0,0,0} };
+  double home_tol_{0.02};
+  double home_hold_time_{0.3};
+  double home_hold_acc_{0.0};
+
+  // IBVS
+  double lambda_{0.7};
+  double tag_half_{0.06};
+  double Z_des_{0.60};
+
+  double mu_{0.02};
+  double qdot_max_{0.6};
+  double vmax6_[6] = {0.15, 0.15, 0.20, 0.6, 0.6, 0.6};
+
+  int step_count_{0};
+
+  vpServo task_;
+  vpPoint obj_pts_[4];
+  vpFeaturePoint s_cur_[4];
+  vpFeaturePoint s_star_[4];
+
+  std::array<double,7> q_cmd_{ {0,0,0,0,0,0,0} };
+};
+
+
+
+
+
+
+// ---- ViSP ----
+#include <visp3/core/vpCameraParameters.h>
+#include <visp3/core/vpImage.h>
+
+#include <visp3/detection/vpDetectorAprilTag.h>
+
+
+class IBVSFromMujocoCameraViewer : public MuJocoViewer {
+public:
+  using MuJocoViewer::MuJocoViewer;
+
+protected:
+  enum class Mode { HOMING, IBVS };
+
+  void initOnce(mjModel* m, mjData* d) override {
+    // 1) 主视角 + PiP（你的 renderPiP 会自动把 hand_cam 画出来并缓存 RGBD）
+    setupCamera(3.0, -170.0, -40.0);
+    enablePiPCamera("hand_cam", 405, 1000, 320, 240);
+
+    // 2) 右臂 7DOF actuator / joint
+    const char* act_names[7] = {
+      "R_SHOULDER_P_pos",
+      "R_SHOULDER_R_pos",
+      "R_SHOULDER_Y_pos",
+      "R_ELBOW_R_pos",
+      "R_WRIST_P_pos",
+      "R_WRIST_Y_pos",
+      "R_WRIST_R_pos"
+    };
+    const char* jnt_names[7] = {
+      "R_SHOULDER_P",
+      "R_SHOULDER_R",
+      "R_SHOULDER_Y",
+      "R_ELBOW_R",
+      "R_WRIST_P",
+      "R_WRIST_Y",
+      "R_WRIST_R"
+    };
+
+    for (int i = 0; i < 7; ++i) {
+      act_ids_[i] = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]);
+      jnt_ids_[i] = mj_name2id(m, mjOBJ_JOINT,    jnt_names[i]);
+      if (act_ids_[i] < 0) {
+        std::cout << "[IBVS] Cannot find actuator " << act_names[i] << std::endl;
+      }
+      if (jnt_ids_[i] < 0) {
+        std::cout << "[IBVS] Cannot find joint " << jnt_names[i] << std::endl;
+      }
+
+      if (jnt_ids_[i] >= 0) {
+        qpos_adr_[i] = m->jnt_qposadr[jnt_ids_[i]];
+        dof_adr_[i]  = m->jnt_dofadr[jnt_ids_[i]];
+      }
+    }
+
+    // 3) 必须有：R_CAM_SITE 用来算 jacobian（末端相机坐标）
+    cam_site_id_ = mj_name2id(m, mjOBJ_SITE, "R_CAM_SITE");
+    if (cam_site_id_ < 0) {
+      std::cout << "[IBVS] Missing site R_CAM_SITE in XML" << std::endl;
+      ready_ = false;
+      return;
+    }
+
+    // 4) hand_cam：用它的 fovy 计算内参
+    hand_cam_id_ = mj_name2id(m, mjOBJ_CAMERA, "hand_cam");
+    if (hand_cam_id_ < 0) {
+      std::cout << "[IBVS] Missing camera hand_cam in XML" << std::endl;
+      ready_ = false;
+      return;
+    }
+
+    // 5) homing 姿态（先让相机看到 tag）
+    q_home_ = { 0.25, 1.00, M_PI/2 - 0.2, M_PI/2 - 0.2, -M_PI + 0.5, 0.0, 0.0 };
+
+    // 6) 坐标对齐（site相机系 -> ViSP相机系）
+    // 这就是你之前验证过能收敛的那套：x/y 同时翻转
+    R_cv_ = (Eigen::Matrix3d() <<
+      -1, 0, 0,
+       0,-1, 0,
+       0, 0, 1).finished();
+
+    // 7) IBVS 任务：用 AprilTag 的 4 个角点特征
+    tag_size_m_ = 0.12;            // 12cm
+    tag_half_   = tag_size_m_ * 0.5;
+    Z_des_      = 0.60;
+    lambda_     = 0.7;
+
+    task_.setServo(vpServo::EYEINHAND_CAMERA);
+    task_.setInteractionMatrixType(vpServo::CURRENT);
+    task_.setLambda(lambda_);
+
+    // tag 平面 z=0，原点在 tag 中心
+    obj_pts_[0].setWorldCoordinates(-tag_half_, -tag_half_, 0.0);
+    obj_pts_[1].setWorldCoordinates( tag_half_, -tag_half_, 0.0);
+    obj_pts_[2].setWorldCoordinates( tag_half_,  tag_half_, 0.0);
+    obj_pts_[3].setWorldCoordinates(-tag_half_,  tag_half_, 0.0);
+
+    // desired: 正对 + 距离 Z_des
+    {
+      vpTranslationVector t_des(0.0, 0.0, Z_des_);
+      vpRotationMatrix R_des; R_des.eye();
+      vpHomogeneousMatrix cMo_des(t_des, R_des);
+
+      for (int i = 0; i < 4; ++i) {
+        obj_pts_[i].track(cMo_des);
+        s_star_[i].buildFrom(obj_pts_[i].get_x(),
+                             obj_pts_[i].get_y(),
+                             obj_pts_[i].get_Z());
+
+        s_cur_[i].buildFrom(0.0, 0.0, 1.0);
+        task_.addFeature(s_cur_[i], s_star_[i]);
+      }
+    }
+
+    // 8) AprilTag detector
+    detector_ = vpDetectorAprilTag(vpDetectorAprilTag::TAG_36h11);
+    detector_.setAprilTagPoseEstimationMethod(vpDetectorAprilTag::HOMOGRAPHY_VIRTUAL_VS);
+
+    // 初始化 q_cmd，防跳变
+    for (int i = 0; i < 7; ++i) {
+      q_cmd_[i] = (qpos_adr_[i] >= 0) ? d->qpos[qpos_adr_[i]] : 0.0;
+    }
+
+    mode_ = Mode::HOMING;
+    home_hold_acc_ = 0.0;
+    step_count_ = 0;
+    last_frame_id_ = 0;
+    ready_ = true;
+
+    std::cout << "[IBVS] initOnce OK. cam_site_id=" << cam_site_id_
+              << " hand_cam_id=" << hand_cam_id_ << std::endl;
+  }
+
+  void controlCallback(mjModel* m, mjData* d) override {
+    if (!ready_) return;
+    const double dt = m->opt.timestep;
+
+    // ======================
+    // A) HOMING
+    // ======================
+    if (mode_ == Mode::HOMING) {
+      double max_err = 0.0;
+      for (int i = 0; i < 7; ++i) {
+        if (act_ids_[i] < 0 || qpos_adr_[i] < 0) continue;
+        d->ctrl[act_ids_[i]] = q_home_[i];
+        const double qi = d->qpos[qpos_adr_[i]];
+        max_err = std::max(max_err, std::abs(qi - q_home_[i]));
+      }
+
+      if (max_err < home_tol_) home_hold_acc_ += dt;
+      else home_hold_acc_ = 0.0;
+
+      if (home_hold_acc_ > home_hold_time_) {
+        for (int i = 0; i < 7; ++i) {
+          if (qpos_adr_[i] >= 0) q_cmd_[i] = d->qpos[qpos_adr_[i]];
+        }
+        mode_ = Mode::IBVS;
+        std::cout << "[IBVS] switch HOMING -> IBVS" << std::endl;
+      }
+      return;
+    }
+
+    // ======================
+    // B) IBVS（用 MuJoCo 相机图像）
+    // ======================
+
+    // 1) 取最新 PiP 图像（RGB + depth z-buffer）
+    std::vector<unsigned char> rgb;
+    std::vector<float> depth;
+    int w=0, h=0;
+    uint64_t fid=0;
+
+    if (!getPiPCameraRGBD(rgb, depth, w, h, fid)) {
+      return;
+    }
+    if (fid == last_frame_id_) {
+      return; // 没新帧就不做视觉（避免重复计算）
+    }
+    last_frame_id_ = fid;
+
+    if (w <= 0 || h <= 0 || (int)rgb.size() != 3*w*h) return;
+
+    // 2) 计算相机内参（从 hand_cam 的 fovy）
+    // MuJoCo 的 fovy 是“垂直视场角（度）”
+    const double fovy_deg = m->cam_fovy[hand_cam_id_];
+    const double fovy = fovy_deg * M_PI / 180.0;
+    const double fy = (h * 0.5) / std::tan(fovy * 0.5);
+    const double fx = fy;                 // 由几何关系可得（见推导）
+    const double cx = w * 0.5;
+    const double cy = h * 0.5;
+
+    vpCameraParameters cam;
+    cam.initPersProjWithoutDistortion(fx, fy, cx, cy);
+
+    // 3) RGB -> 灰度 vpImage
+    vpImage<unsigned char> I(h, w);
+    for (int y = 0; y < h; ++y) {
+      for (int x = 0; x < w; ++x) {
+        const int idx = (y*w + x) * 3;
+        const unsigned char r = rgb[idx + 0];
+        const unsigned char g = rgb[idx + 1];
+        const unsigned char b = rgb[idx + 2];
+        I[y][x] = static_cast<unsigned char>(0.299*r + 0.587*g + 0.114*b);
+      }
+    }
+
+    // 4) AprilTag 检测 + 位姿估计（直接得到 cMo）
+    std::vector<vpHomogeneousMatrix> cMo_vec;
+    bool ok = detector_.detect(I, tag_size_m_, cam, cMo_vec);
+
+    if (!ok || cMo_vec.empty()) {
+      // 没检测到：最简单策略：保持当前位置（不更新 ctrl）
+      if ((step_count_ % 60) == 0) {
+        std::cout << "[IBVS] no tag detected" << std::endl;
+      }
+      ++step_count_;
+      return;
+    }
+
+    vpHomogeneousMatrix cMo = cMo_vec[0];
+
+    // 5) 用 cMo 更新四角点特征
+    for (int i = 0; i < 4; ++i) {
+      obj_pts_[i].track(cMo);
+      const double x = obj_pts_[i].get_x();
+      const double y = obj_pts_[i].get_y();
+      const double Z = std::max(obj_pts_[i].get_Z(), 0.05);
+      s_cur_[i].buildFrom(x, y, Z);
+    }
+
+    // 6) ViSP 控制律：得到相机速度 v_c（ViSP 相机系）
+    vpColVector v_c = task_.computeControlLaw();
+
+    // 限幅（很关键）
+    for (int k = 0; k < 6; ++k) v_c[k] = clamp(v_c[k], -vmax6_[k], vmax6_[k]);
+
+    if ((step_count_ % 60) == 0) {
+      // 粗略打印一下位姿平移（单位 m）
+      vpTranslationVector t = cMo.getTranslationVector();
+      std::cout << "[IBVS] t_co(visp)=[" << t[0] << " " << t[1] << " " << t[2]
+                << "]" << std::endl;
+      std::cout << "[IBVS] v_c=[" << v_c[0] << " " << v_c[1] << " " << v_c[2]
+                << " " << v_c[3] << " " << v_c[4] << " " << v_c[5]
+                << "]" << std::endl;
+    }
+
+    // 7) 把 ViSP 相机速度 -> site 相机速度 -> world twist
+    // 先：ViSP -> site（逆对齐）
+    Eigen::Vector3d v_visp(v_c[0], v_c[1], v_c[2]);
+    Eigen::Vector3d w_visp(v_c[3], v_c[4], v_c[5]);
+    Eigen::Vector3d v_site = R_cv_.transpose() * v_visp;
+    Eigen::Vector3d w_site = R_cv_.transpose() * w_visp;
+
+    // 再：site -> world（用 R_cw）
+    const mjtNum* pc = d->site_xpos + 3 * cam_site_id_;
+    const mjtNum* Rc9 = d->site_xmat + 9 * cam_site_id_;
+    (void)pc;
+    Eigen::Matrix3d R_cw = xmat_to_R(Rc9);
+
+    Eigen::Vector3d v_w = R_cw * v_site;
+    Eigen::Vector3d w_w = R_cw * w_site;
+
+    Eigen::Matrix<double,6,1> twist_w;
+    twist_w << v_w(0), v_w(1), v_w(2), w_w(0), w_w(1), w_w(2);
+
+    // 8) Jacobian（mj_jacSite 给的是 world 线速度/角速度）
+    std::vector<mjtNum> jacp(3 * m->nv);
+    std::vector<mjtNum> jacr(3 * m->nv);
+    mj_jacSite(m, d, jacp.data(), jacr.data(), cam_site_id_);
+
+    Eigen::Matrix<double,6,7> J;
+    J.setZero();
+    for (int j = 0; j < 7; ++j) {
+      const int dof = dof_adr_[j];
+      if (dof < 0) continue;
+      J(0,j) = jacp[0*m->nv + dof];
+      J(1,j) = jacp[1*m->nv + dof];
+      J(2,j) = jacp[2*m->nv + dof];
+      J(3,j) = jacr[0*m->nv + dof];
+      J(4,j) = jacr[1*m->nv + dof];
+      J(5,j) = jacr[2*m->nv + dof];
+    }
+
+    // 9) DLS: qdot
+    Eigen::Matrix<double,6,6> A = J * J.transpose();
+    A += (mu_*mu_) * Eigen::Matrix<double,6,6>::Identity();
+    Eigen::Matrix<double,7,1> qdot = J.transpose() * A.inverse() * twist_w;
+
+    for (int i = 0; i < 7; ++i) qdot(i) = clamp(qdot(i), -qdot_max_, qdot_max_);
+
+    // 10) integrate -> 位置控制 ctrl
+    for (int i = 0; i < 7; ++i) {
+      q_cmd_[i] += qdot(i) * dt;
+
+      // joint limit clamp
+      if (jnt_ids_[i] >= 0 && m->jnt_limited[jnt_ids_[i]]) {
+        const double lo = m->jnt_range[2*jnt_ids_[i] + 0];
+        const double hi = m->jnt_range[2*jnt_ids_[i] + 1];
+        q_cmd_[i] = clamp(q_cmd_[i], lo, hi);
+      }
+
+      if (act_ids_[i] >= 0) d->ctrl[act_ids_[i]] = q_cmd_[i];
+    }
+
+    ++step_count_;
+  }
+
+  void onReset(mjModel* m, mjData* d) override {
+    (void)m;
+    for (int i = 0; i < 7; ++i) {
+      if (qpos_adr_[i] >= 0) q_cmd_[i] = d->qpos[qpos_adr_[i]];
+    }
+    mode_ = Mode::HOMING;
+    home_hold_acc_ = 0.0;
+    step_count_ = 0;
+    last_frame_id_ = 0;
+  }
+
+private:
+  bool ready_{false};
+
+  // ids
+  std::array<int,7> act_ids_{};
+  std::array<int,7> jnt_ids_{};
+  std::array<int,7> qpos_adr_{ { -1,-1,-1,-1,-1,-1,-1 } };
+  std::array<int,7> dof_adr_{  { -1,-1,-1,-1,-1,-1,-1 } };
+  int cam_site_id_{-1};
+  int hand_cam_id_{-1};
+
+  // homing
+  Mode mode_{Mode::HOMING};
+  std::array<double,7> q_home_{ {0,0,0,0,0,0,0} };
+  double home_tol_{0.02};
+  double home_hold_time_{0.3};
+  double home_hold_acc_{0.0};
+
+  // align: site -> ViSP
+  Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()};
+
+  // IBVS params
+  double lambda_{0.7};
+  double tag_size_m_{0.12};
+  double tag_half_{0.06};
+  double Z_des_{0.60};
+
+  double mu_{0.02};          // DLS damping
+  double qdot_max_{0.6};     // rad/s
+  double vmax6_[6] = {0.15, 0.15, 0.20, 0.6, 0.6, 0.6};
+
+  // ViSP
+  vpServo task_;
+  vpPoint obj_pts_[4];
+  vpFeaturePoint s_cur_[4];
+  vpFeaturePoint s_star_[4];
+  vpDetectorAprilTag detector_;
+
+  // control state
+  std::array<double,7> q_cmd_{ {0,0,0,0,0,0,0} };
+  int step_count_{0};
+  uint64_t last_frame_id_{0};
+};
+
+
+
+
+
+class ControllerViewer : public MuJocoViewer {
+public:
+  using MuJocoViewer::MuJocoViewer;
+
+  void setTarget(const std::vector<double> &q_target) {
+    std::lock_guard<std::mutex> lock(mtx_);
+    q_cmd_ = q_target;
+  }
+
+protected:
+  void initOnce(mjModel *m, mjData *d) override
+  {
+    (void)d;
+    setupCamera(3.0, -170.0, -40.0);
+    // enablePiPCamera("hand_cam", 320, 240, 10);
+    enablePiPCamera("hand_cam", 405, 1200, 320, 240);
+
+    const char *act_names[7] = {
+      "R_SHOULDER_P_pos",
+      "R_SHOULDER_R_pos",
+      "R_SHOULDER_Y_pos",
+      "R_ELBOW_R_pos",
+      "R_WRIST_P_pos",
+      "R_WRIST_Y_pos",
+      "R_WRIST_R_pos"
+    };
+
+    for (int i = 0; i < 7; ++i) {
+      act_ids_[i] = mj_name2id(m, mjOBJ_ACTUATOR, act_names[i]);
+      if (act_ids_[i] < 0) {
+        std::cout << "Cannot find actuator " << act_names[i] << std::endl;
+      }
+    }
+
+    act_ids_inited_ = true;
+  }
+
+  void controlCallback(mjModel *m, mjData *d) override
+  {
+    (void)m;
+    if (!act_ids_inited_) return;
+
+    std::vector<double> q_local;
+    {
+      std::lock_guard<std::mutex> lock(mtx_);
+      q_local = q_cmd_;
+    }
+
+    for (int i = 0; i < 7; ++i) {
+      if (act_ids_[i] < 0) continue;
+      if (i < static_cast<int>(q_local.size())) {
+        d->ctrl[act_ids_[i]] = q_local[i];
+      }
+    }
+  }
+
+  void onReset(mjModel *m, mjData *d) override
+  {
+    (void)m;
+    (void)d;
+    std::lock_guard<std::mutex> lock(mtx_);
+    q_cmd_.assign(7, 0.0);
+  }
+
+private:
+  std::array<int, 7> act_ids_{};
+  bool act_ids_inited_{false};
+  std::vector<double> q_cmd_{7, 0.0};
+  std::mutex mtx_;
+};
+
+TEST(controller_test, mujoco_viewer_smoke)
+{
+  ControllerViewer viewer("/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml");
+
+  std::vector<double> q_seed = {
+    0.25, 1.00, M_PI / 2 - 0.2, M_PI / 2 - 0.2, -M_PI+ 0.5 , 0, 0
+  };
+  viewer.setTarget(q_seed);
+  viewer.run();
+}
+
+TEST(controller_test, mujoco_ibvs_sim_gt)
+{
+  IBVSHomingViewer viewer("/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml");
+  viewer.run();
+}
+
+TEST(controller_test, mujoco_camera_apriltag_ibvs_full)
+{
+  IBVSFromMujocoCameraViewer viewer(
+    "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.xml"
+  );
+  viewer.run();
+}

cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h

@@ -4,16 +4,20 @@
 
 #pragma once
 
-#include <memory>
-#include <thread>
 #include <atomic>
+#include <memory>
+#include <mutex>
+#include <thread>
+#include <vector>
 
 #include "mujoco/mujoco.h"
 #include "simulate/mujoco/mujoco_viewer/include/simulate.h"
 #include "common/consts/constant.h"
 
 namespace cmvr {
+    class PiPGlfwAdapter;
     class MuJocoViewer {
+        friend class PiPGlfwAdapter;
     public:
         explicit MuJocoViewer(const char *model_path);
 
@@ -29,6 +33,24 @@ namespace cmvr {
         mjModel *model() const { return m_; }
         mjData *data() const { return d_; }
 
+        // 同一窗口画中画：显示模型内固定相机视角（像素坐标）
+        void enablePiPCamera(const char *camera_name,
+                             int left,
+                             int bottom,
+                             int width,
+                             int height);
+        void disablePiPCamera();
+        // 获取 PiP 相机 RGB+Depth（Depth 是 OpenGL z-buffer 0..1）
+        // depth 可不取（传 nullptr 或者用 getPiPCameraRGB 旧接口）
+        bool getPiPCameraRGBD(std::vector<unsigned char> &rgb,
+                              std::vector<float> &depth,
+                              int &width,
+                              int &height,
+                              uint64_t &frame_id) const;
+
+        // 只拿 frame_id，便于 physics 线程判断是否新帧
+        uint64_t getPiPCameraFrameId() const;
+
     protected:
         // 每次 mj_step 前，physics 线程回调控制逻辑
         virtual void controlCallback(mjModel *m, mjData *d) {
@@ -59,6 +81,7 @@ namespace cmvr {
         void printCameraState() const;
 
     private:
+        void renderPiP();
         void initSim(); // 只创建 Simulate，不 load
         void physicsThreadFunc(); // 加载模型 + 物理循环
         void physicsLoop(); // 真正的一步一步仿真
@@ -87,6 +110,26 @@ namespace cmvr {
         bool inited_ = false;
         mjtNum last_time_ = 0.0;
 
+        bool pip_enabled_ = false;
+        std::string pip_camera_name_;
+        int pip_camera_id_ = -1;
+        int pip_width_ = 320;
+        int pip_height_ = 240;
+        int pip_margin_ = 10;
+        bool pip_custom_pos_ = false;
+        int pip_left_ = 0;
+        int pip_bottom_ = 0;
+        mjvCamera pip_cam_;
+        mjvScene pip_scene_;
+        bool pip_scene_inited_ = false;
+        mjModel *pip_scene_model_ = nullptr;
+        mutable std::mutex pip_rgb_mtx_;
+        std::vector<unsigned char> pip_rgb_;
+        std::vector<float> pip_depth_;     // 新增：z-buffer
+        int pip_rgb_width_ = 0;
+        int pip_rgb_height_ = 0;
+        bool pip_rgb_valid_ = false;
+        uint64_t pip_frame_id_ = 0;        // 新增：帧序号
 
     };
 } // namespace cmvr

cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp

@@ -3,6 +3,7 @@
 //
 
 
+#include <algorithm>
 #include <cstdio>
 #include <chrono>
 #include <cmath>
@@ -18,6 +19,23 @@ namespace cmvr {
 
     constexpr double kSyncMisalign = 0.1;
     constexpr double kSimRefreshFraction = 0.7;
+    constexpr int kPiPMaxGeom = 100000;
+
+    class PiPGlfwAdapter : public mj::GlfwAdapter {
+    public:
+        explicit PiPGlfwAdapter(MuJocoViewer *owner)
+            : owner_(owner) {}
+
+        void SwapBuffers() override {
+            if (owner_) {
+                owner_->renderPiP();
+            }
+            mj::GlfwAdapter::SwapBuffers();
+        }
+
+    private:
+        MuJocoViewer *owner_;
+    };
 
 
     static const char *CheckDiverged(int disableflags, const mjData *d) {
@@ -76,8 +94,10 @@ namespace cmvr {
         mjv_defaultCamera(&cam_);
         mjv_defaultOption(&opt_);
         mjv_defaultPerturb(&pert_);
+        mjv_defaultCamera(&pip_cam_);
+        mjv_defaultScene(&pip_scene_);
 
-        auto platform_ui = std::make_unique<mj::GlfwAdapter>();
+        auto platform_ui = std::make_unique<PiPGlfwAdapter>(this);
         sim_ = std::make_unique<mj::Simulate>(
             std::move(platform_ui),
             &cam_, &opt_, &pert_,
@@ -93,6 +113,11 @@ namespace cmvr {
             physics_thread_.join();
         }
 
+        if (pip_scene_inited_) {
+            mjv_freeScene(&pip_scene_);
+            pip_scene_inited_ = false;
+        }
+
         if (d_) mj_deleteData(d_);
         if (m_) mj_deleteModel(m_);
     }
@@ -155,6 +180,117 @@ namespace cmvr {
         cam_.elevation = elevation; // 俯仰角（度）
     }
 
+    void MuJocoViewer::enablePiPCamera(const char *camera_name,
+                                       int left,
+                                       int bottom,
+                                       int width,
+                                       int height) {
+        pip_enabled_ = true;
+        pip_camera_name_ = camera_name ? camera_name : "";
+        pip_camera_id_ = -1;
+        pip_left_ = left;
+        pip_bottom_ = bottom;
+        pip_width_ = width > 0 ? width : 320;
+        pip_height_ = height > 0 ? height : 240;
+        pip_custom_pos_ = true;
+    }
+
+    void MuJocoViewer::disablePiPCamera() {
+        pip_enabled_ = false;
+    }
+
+
+
+    void MuJocoViewer::renderPiP() {
+        if (!pip_enabled_ || !sim_ || !m_ || !d_) return;
+        if (pip_camera_name_.empty()) return;
+
+        if (pip_camera_id_ < 0) {
+            pip_camera_id_ = mj_name2id(m_, mjOBJ_CAMERA, pip_camera_name_.c_str());
+            if (pip_camera_id_ < 0) {
+                return;
+            }
+        }
+
+        auto [fb_width, fb_height] = sim_->platform_ui->GetFramebufferSize();
+        if (fb_width <= 0 || fb_height <= 0) return;
+
+        int left = 0;
+        int bottom = 0;
+        int width = 0;
+        int height = 0;
+
+        if (pip_custom_pos_) {
+            left = std::max(0, std::min(pip_left_, fb_width - 1));
+            bottom = std::max(0, std::min(pip_bottom_, fb_height - 1));
+            width = std::min(pip_width_, fb_width - left);
+            height = std::min(pip_height_, fb_height - bottom);
+        } else {
+            width = std::min(pip_width_, fb_width - 2 * pip_margin_);
+            height = std::min(pip_height_, fb_height - 2 * pip_margin_);
+            left = fb_width - pip_margin_ - width;
+            bottom = pip_margin_;
+        }
+
+        if (width <= 0 || height <= 0) return;
+
+        mjrRect rect;
+        rect.width = width;
+        rect.height = height;
+        rect.left = left;
+        rect.bottom = bottom;
+
+        const std::unique_lock<std::recursive_mutex> lock(sim_->mtx);
+
+        if (!pip_scene_inited_ || pip_scene_model_ != m_) {
+            if (pip_scene_inited_) {
+                mjv_freeScene(&pip_scene_);
+            }
+            mjv_makeScene(m_, &pip_scene_, kPiPMaxGeom);
+            pip_scene_inited_ = true;
+            pip_scene_model_ = m_;
+        }
+
+        pip_cam_.type = mjCAMERA_FIXED;
+        pip_cam_.fixedcamid = pip_camera_id_;
+        pip_cam_.trackbodyid = -1;
+
+        mjv_updateScene(m_, d_, &opt_, &pert_, &pip_cam_, mjCAT_ALL, &pip_scene_);
+        mjr_render(rect, &pip_scene_, &sim_->platform_ui->mjr_context());
+
+        {
+            std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
+            const int w = rect.width;
+            const int h = rect.height;
+            if (w > 0 && h > 0) {
+                pip_rgb_.resize(static_cast<size_t>(3 * w * h));
+                pip_depth_.resize(static_cast<size_t>(w * h));
+
+                // 同时读 RGB 和 depth(z-buffer 0..1)
+                mjr_readPixels(pip_rgb_.data(), pip_depth_.data(),
+                               rect, &sim_->platform_ui->mjr_context());
+
+                // OpenGL 像素原点在左下，需要竖直翻转 RGB 和 depth
+                for (int r = 0; r < h / 2; ++r) {
+                    // flip rgb row
+                    unsigned char *top_row = pip_rgb_.data() + 3 * w * r;
+                    unsigned char *bottom_row = pip_rgb_.data() + 3 * w * (h - 1 - r);
+                    std::swap_ranges(top_row, top_row + 3 * w, bottom_row);
+
+                    // flip depth row
+                    float *top_d = pip_depth_.data() + w * r;
+                    float *bot_d = pip_depth_.data() + w * (h - 1 - r);
+                    std::swap_ranges(top_d, top_d + w, bot_d);
+                }
+
+                pip_rgb_width_ = w;
+                pip_rgb_height_ = h;
+                pip_rgb_valid_ = true;
+                ++pip_frame_id_;   // 新帧
+            }
+        }
+    }
+
     void MuJocoViewer::physicsLoop() {
         using Clock = mj::Simulate::Clock;
 
@@ -288,4 +424,26 @@ namespace cmvr {
             physics_thread_.join();
         }
     }
+
+
+    uint64_t MuJocoViewer::getPiPCameraFrameId() const {
+        std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
+        return pip_frame_id_;
+    }
+
+    bool MuJocoViewer::getPiPCameraRGBD(std::vector<unsigned char> &rgb,
+                                        std::vector<float> &depth,
+                                        int &width,
+                                        int &height,
+                                        uint64_t &frame_id) const {
+        std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
+        if (!pip_rgb_valid_ || pip_rgb_.empty()) return false;
+
+        rgb = pip_rgb_;
+        depth = pip_depth_;
+        width = pip_rgb_width_;
+        height = pip_rgb_height_;
+        frame_id = pip_frame_id_;
+        return true;
+    }
 } // namespace cmvr

dependency/x86/third_party/visp/3.7.0/bin/visp-config

@@ -0,0 +1,98 @@
+#!/bin/sh
+
+#############################################################################
+#
+# ViSP, open source Visual Servoing Platform software.
+# Copyright (C) 2005 - 2025 by Inria. All rights reserved.
+#
+# This software is free software; you can redistribute it and/or modify
+# it under the terms of the GNU General Public License as published by
+# the Free Software Foundation; either version 2 of the License, or
+# (at your option) any later version.
+# See the file LICENSE.txt at the root directory of this source
+# distribution for additional information about the GNU GPL.
+#
+# For using ViSP with software that can not be combined with the GNU
+# GPL, please contact Inria about acquiring a ViSP Professional
+# Edition License.
+#
+# See https://visp.inria.fr for more information.
+#
+# This software was developed at:
+# Inria Rennes - Bretagne Atlantique
+# Campus Universitaire de Beaulieu
+# 35042 Rennes Cedex
+# France
+#
+# If you have questions regarding the use of this file, please contact
+# Inria at visp@inria.fr
+#
+# This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
+# WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
+#
+# Description:
+# visp-config shell script.
+# Auto-generated from visp-config.install.in by cmake. For backward
+# compatibility used visp.pc.
+#
+#############################################################################
+
+
+relpath=`dirname $0`
+relpath=`(cd $relpath/..; pwd)`
+
+PREFIX=$relpath
+
+CFLAGS_CMD=`pkg-config --cflags visp`
+CFLAGS=$CFLAGS_CMD
+
+LIBS_CMD=`pkg-config --libs visp`
+LIBS=$LIBS_CMD
+
+VERSION_CMD=`pkg-config --modversion visp`
+VERSION=$VERSION_CMD
+
+NO_NEWLINE_CHARACTER=""
+NO_NEWLINE_OPTION=""
+
+usage()
+{
+    cat <<EOF
+
+ViSP $VERSION (Visual Servoing Platform)
+Copyright (C) 2005 - 2023 Inria. All rights reserved.
+
+Usage: $0 [--prefix] [--cflags] [--libs] [--version] [--dumpversion]  [--help]
+
+  --prefix      Show ViSP installation prefix.
+  --cflags      Print pre-processor and compiler flags including
+                third party includes we depend on.
+  --libs        Print library linking information with ViSP
+                including third party libraries we depend on.
+  --version     Output ViSP information.
+  --dumpversion Output ViSP version information.
+  --help        Display this help and exit.
+
+EOF
+    exit $1
+}
+
+if ! test "$1"; then
+    usage;
+    exit 0;
+fi;
+
+for arg in $@; do
+    case $arg in
+  --prefix) echo $NO_NEWLINE_OPTION "$PREFIX$NO_NEWLINE_CHARACTER";;
+  --cflags) echo $NO_NEWLINE_OPTION "$CFLAGS$NO_NEWLINE_CHARACTER";;
+  --libs) echo $NO_NEWLINE_OPTION "$LIBS$NO_NEWLINE_CHARACTER";;
+  --version)
+      echo "ViSP $VERSION (Visual Servoing Platform)"
+      echo ""
+      echo "Copyright (C) 2005 - 2023 Inria. All rights reserved.";;
+  --dumpversion) echo $NO_NEWLINE_OPTION "$VERSION$NO_NEWLINE_CHARACTER";;
+        *) usage; exit 0 ;;
+    esac;
+done;
+echo ""

dependency/x86/third_party/visp/3.7.0/include/visp/visp_modules.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __visp_modules_gen_h_
+#define __visp_modules_gen_h_
+
+#include <visp3/visp_modules.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vp1394CMUGrabber.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vp1394CMUGrabber_gen_h_
+#define __vp1394CMUGrabber_gen_h_
+
+#include <visp3/sensor/vp1394CMUGrabber.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vp1394TwoGrabber.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vp1394TwoGrabber_gen_h_
+#define __vp1394TwoGrabber_gen_h_
+
+#include <visp3/sensor/vp1394TwoGrabber.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpAR.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpAR_gen_h_
+#define __vpAR_gen_h_
+
+#include <visp3/ar/vpAR.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpAROgre.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpAROgre_gen_h_
+#define __vpAROgre_gen_h_
+
+#include <visp3/ar/vpAROgre.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpAdaptiveGain.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpAdaptiveGain_gen_h_
+#define __vpAdaptiveGain_gen_h_
+
+#include <visp3/vs/vpAdaptiveGain.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpAfma6.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpAfma6_gen_h_
+#define __vpAfma6_gen_h_
+
+#include <visp3/robot/vpAfma6.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpArray2D.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpArray2D_gen_h_
+#define __vpArray2D_gen_h_
+
+#include <visp3/core/vpArray2D.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpBSpline.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpBSpline_gen_h_
+#define __vpBSpline_gen_h_
+
+#include <visp3/core/vpBSpline.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpBasicFeature.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpBasicFeature_gen_h_
+#define __vpBasicFeature_gen_h_
+
+#include <visp3/visual_features/vpBasicFeature.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpBasicKeyPoint.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpBasicKeyPoint_gen_h_
+#define __vpBasicKeyPoint_gen_h_
+
+#include <visp3/vision/vpBasicKeyPoint.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpBiclops.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpBiclops_gen_h_
+#define __vpBiclops_gen_h_
+
+#include <visp3/robot/vpBiclops.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpCPUFeatures.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpCPUFeatures_gen_h_
+#define __vpCPUFeatures_gen_h_
+
+#include <visp3/core/vpCPUFeatures.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpCalibration.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpCalibration_gen_h_
+#define __vpCalibration_gen_h_
+
+#include <visp3/vision/vpCalibration.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpCalibrationException.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpCalibrationException_gen_h_
+#define __vpCalibrationException_gen_h_
+
+#include <visp3/vision/vpCalibrationException.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpCameraParameters.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpCameraParameters_gen_h_
+#define __vpCameraParameters_gen_h_
+
+#include <visp3/core/vpCameraParameters.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpCannyEdgeDetection.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpCannyEdgeDetection_gen_h_
+#define __vpCannyEdgeDetection_gen_h_
+
+#include <visp3/core/vpCannyEdgeDetection.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpCircle.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpCircle_gen_h_
+#define __vpCircle_gen_h_
+
+#include <visp3/core/vpCircle.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpCircleHoughTransform.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpCircleHoughTransform_gen_h_
+#define __vpCircleHoughTransform_gen_h_
+
+#include <visp3/imgproc/vpCircleHoughTransform.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpClient.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpClient_gen_h_
+#define __vpClient_gen_h_
+
+#include <visp3/core/vpClient.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpColVector.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpColVector_gen_h_
+#define __vpColVector_gen_h_
+
+#include <visp3/core/vpColVector.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpColor.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpColor_gen_h_
+#define __vpColor_gen_h_
+
+#include <visp3/core/vpColor.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpColorBlindFriendlyPalette.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpColorBlindFriendlyPalette_gen_h_
+#define __vpColorBlindFriendlyPalette_gen_h_
+
+#include <visp3/gui/vpColorBlindFriendlyPalette.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpColorDepthConversion.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpColorDepthConversion_gen_h_
+#define __vpColorDepthConversion_gen_h_
+
+#include <visp3/core/vpColorDepthConversion.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpColorGetter.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpColorGetter_gen_h_
+#define __vpColorGetter_gen_h_
+
+#include <visp3/core/vpColorGetter.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpColormap.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpColormap_gen_h_
+#define __vpColormap_gen_h_
+
+#include <visp3/core/vpColormap.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpComedi.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpComedi_gen_h_
+#define __vpComedi_gen_h_
+
+#include <visp3/sensor/vpComedi.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpConfig.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpConfig_gen_h_
+#define __vpConfig_gen_h_
+
+#include <visp3/core/vpConfig.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpContours.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpContours_gen_h_
+#define __vpContours_gen_h_
+
+#include <visp3/imgproc/vpContours.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpConvert.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpConvert_gen_h_
+#define __vpConvert_gen_h_
+
+#include <visp3/core/vpConvert.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpCylinder.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpCylinder_gen_h_
+#define __vpCylinder_gen_h_
+
+#include <visp3/core/vpCylinder.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpD3DRenderer.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpD3DRenderer_gen_h_
+#define __vpD3DRenderer_gen_h_
+
+#include <visp3/gui/vpD3DRenderer.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDebug.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDebug_gen_h_
+#define __vpDebug_gen_h_
+
+#include <visp3/core/vpDebug.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDetectorAprilTag.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDetectorAprilTag_gen_h_
+#define __vpDetectorAprilTag_gen_h_
+
+#include <visp3/detection/vpDetectorAprilTag.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDetectorBase.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDetectorBase_gen_h_
+#define __vpDetectorBase_gen_h_
+
+#include <visp3/detection/vpDetectorBase.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDetectorDNNOpenCV.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDetectorDNNOpenCV_gen_h_
+#define __vpDetectorDNNOpenCV_gen_h_
+
+#include <visp3/detection/vpDetectorDNNOpenCV.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDetectorDataMatrixCode.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDetectorDataMatrixCode_gen_h_
+#define __vpDetectorDataMatrixCode_gen_h_
+
+#include <visp3/detection/vpDetectorDataMatrixCode.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDetectorFace.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDetectorFace_gen_h_
+#define __vpDetectorFace_gen_h_
+
+#include <visp3/detection/vpDetectorFace.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDetectorQRCode.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDetectorQRCode_gen_h_
+#define __vpDetectorQRCode_gen_h_
+
+#include <visp3/detection/vpDetectorQRCode.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDirectShowDevice.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDirectShowDevice_gen_h_
+#define __vpDirectShowDevice_gen_h_
+
+#include <visp3/sensor/vpDirectShowDevice.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDirectShowGrabber.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDirectShowGrabber_gen_h_
+#define __vpDirectShowGrabber_gen_h_
+
+#include <visp3/sensor/vpDirectShowGrabber.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDirectShowGrabberImpl.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDirectShowGrabberImpl_gen_h_
+#define __vpDirectShowGrabberImpl_gen_h_
+
+#include <visp3/sensor/vpDirectShowGrabberImpl.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDirectShowSampleGrabberI.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDirectShowSampleGrabberI_gen_h_
+#define __vpDirectShowSampleGrabberI_gen_h_
+
+#include <visp3/sensor/vpDirectShowSampleGrabberI.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDiskGrabber.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDiskGrabber_gen_h_
+#define __vpDiskGrabber_gen_h_
+
+#include <visp3/io/vpDiskGrabber.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDisplay.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDisplay_gen_h_
+#define __vpDisplay_gen_h_
+
+#include <visp3/core/vpDisplay.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDisplayD3D.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDisplayD3D_gen_h_
+#define __vpDisplayD3D_gen_h_
+
+#include <visp3/gui/vpDisplayD3D.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDisplayException.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDisplayException_gen_h_
+#define __vpDisplayException_gen_h_
+
+#include <visp3/core/vpDisplayException.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDisplayFactory.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDisplayFactory_gen_h_
+#define __vpDisplayFactory_gen_h_
+
+#include <visp3/gui/vpDisplayFactory.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDisplayGDI.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDisplayGDI_gen_h_
+#define __vpDisplayGDI_gen_h_
+
+#include <visp3/gui/vpDisplayGDI.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDisplayGTK.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDisplayGTK_gen_h_
+#define __vpDisplayGTK_gen_h_
+
+#include <visp3/gui/vpDisplayGTK.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDisplayOpenCV.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDisplayOpenCV_gen_h_
+#define __vpDisplayOpenCV_gen_h_
+
+#include <visp3/gui/vpDisplayOpenCV.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDisplayPCL.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDisplayPCL_gen_h_
+#define __vpDisplayPCL_gen_h_
+
+#include <visp3/gui/vpDisplayPCL.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDisplayWin32.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDisplayWin32_gen_h_
+#define __vpDisplayWin32_gen_h_
+
+#include <visp3/gui/vpDisplayWin32.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDisplayX.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDisplayX_gen_h_
+#define __vpDisplayX_gen_h_
+
+#include <visp3/gui/vpDisplayX.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDot.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDot_gen_h_
+#define __vpDot_gen_h_
+
+#include <visp3/blob/vpDot.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpDot2.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpDot2_gen_h_
+#define __vpDot2_gen_h_
+
+#include <visp3/blob/vpDot2.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpEigenConversion.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpEigenConversion_gen_h_
+#define __vpEigenConversion_gen_h_
+
+#include <visp3/core/vpEigenConversion.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpEndian.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpEndian_gen_h_
+#define __vpEndian_gen_h_
+
+#include <visp3/core/vpEndian.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpException.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpException_gen_h_
+#define __vpException_gen_h_
+
+#include <visp3/core/vpException.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpExponentialMap.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpExponentialMap_gen_h_
+#define __vpExponentialMap_gen_h_
+
+#include <visp3/core/vpExponentialMap.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureBuilder.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureBuilder_gen_h_
+#define __vpFeatureBuilder_gen_h_
+
+#include <visp3/visual_features/vpFeatureBuilder.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureDepth.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureDepth_gen_h_
+#define __vpFeatureDepth_gen_h_
+
+#include <visp3/visual_features/vpFeatureDepth.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureDisplay.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureDisplay_gen_h_
+#define __vpFeatureDisplay_gen_h_
+
+#include <visp3/core/vpFeatureDisplay.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureEllipse.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureEllipse_gen_h_
+#define __vpFeatureEllipse_gen_h_
+
+#include <visp3/visual_features/vpFeatureEllipse.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureException.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureException_gen_h_
+#define __vpFeatureException_gen_h_
+
+#include <visp3/visual_features/vpFeatureException.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureLine.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureLine_gen_h_
+#define __vpFeatureLine_gen_h_
+
+#include <visp3/visual_features/vpFeatureLine.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureLuminance.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureLuminance_gen_h_
+#define __vpFeatureLuminance_gen_h_
+
+#include <visp3/visual_features/vpFeatureLuminance.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureLuminanceMapping.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureLuminanceMapping_gen_h_
+#define __vpFeatureLuminanceMapping_gen_h_
+
+#include <visp3/visual_features/vpFeatureLuminanceMapping.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureMoment.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureMoment_gen_h_
+#define __vpFeatureMoment_gen_h_
+
+#include <visp3/visual_features/vpFeatureMoment.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureMomentAlpha.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureMomentAlpha_gen_h_
+#define __vpFeatureMomentAlpha_gen_h_
+
+#include <visp3/visual_features/vpFeatureMomentAlpha.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureMomentArea.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureMomentArea_gen_h_
+#define __vpFeatureMomentArea_gen_h_
+
+#include <visp3/visual_features/vpFeatureMomentArea.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureMomentAreaNormalized.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureMomentAreaNormalized_gen_h_
+#define __vpFeatureMomentAreaNormalized_gen_h_
+
+#include <visp3/visual_features/vpFeatureMomentAreaNormalized.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureMomentBasic.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureMomentBasic_gen_h_
+#define __vpFeatureMomentBasic_gen_h_
+
+#include <visp3/visual_features/vpFeatureMomentBasic.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureMomentCInvariant.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureMomentCInvariant_gen_h_
+#define __vpFeatureMomentCInvariant_gen_h_
+
+#include <visp3/visual_features/vpFeatureMomentCInvariant.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureMomentCentered.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureMomentCentered_gen_h_
+#define __vpFeatureMomentCentered_gen_h_
+
+#include <visp3/visual_features/vpFeatureMomentCentered.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureMomentCommon.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureMomentCommon_gen_h_
+#define __vpFeatureMomentCommon_gen_h_
+
+#include <visp3/visual_features/vpFeatureMomentCommon.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureMomentDatabase.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureMomentDatabase_gen_h_
+#define __vpFeatureMomentDatabase_gen_h_
+
+#include <visp3/visual_features/vpFeatureMomentDatabase.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureMomentGravityCenter.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureMomentGravityCenter_gen_h_
+#define __vpFeatureMomentGravityCenter_gen_h_
+
+#include <visp3/visual_features/vpFeatureMomentGravityCenter.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureMomentGravityCenterNormalized.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureMomentGravityCenterNormalized_gen_h_
+#define __vpFeatureMomentGravityCenterNormalized_gen_h_
+
+#include <visp3/visual_features/vpFeatureMomentGravityCenterNormalized.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeaturePoint.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeaturePoint_gen_h_
+#define __vpFeaturePoint_gen_h_
+
+#include <visp3/visual_features/vpFeaturePoint.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeaturePoint3D.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeaturePoint3D_gen_h_
+#define __vpFeaturePoint3D_gen_h_
+
+#include <visp3/visual_features/vpFeaturePoint3D.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeaturePointPolar.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeaturePointPolar_gen_h_
+#define __vpFeaturePointPolar_gen_h_
+
+#include <visp3/visual_features/vpFeaturePointPolar.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureSegment.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureSegment_gen_h_
+#define __vpFeatureSegment_gen_h_
+
+#include <visp3/visual_features/vpFeatureSegment.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureThetaU.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureThetaU_gen_h_
+#define __vpFeatureThetaU_gen_h_
+
+#include <visp3/visual_features/vpFeatureThetaU.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureTranslation.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureTranslation_gen_h_
+#define __vpFeatureTranslation_gen_h_
+
+#include <visp3/visual_features/vpFeatureTranslation.h>
+
+#endif
+

dependency/x86/third_party/visp/3.7.0/include/visp/vpFeatureVanishingPoint.h

@@ -0,0 +1,12 @@
+/*
+ *      ** File generated automatically, do not modify **
+ *
+*/
+
+#ifndef __vpFeatureVanishingPoint_gen_h_
+#define __vpFeatureVanishingPoint_gen_h_
+
+#include <visp3/visual_features/vpFeatureVanishingPoint.h>
+
+#endif
+