Merge branch 'master' of github.com:MarcToussaint/botop

README.md

@@ -34,7 +34,7 @@ This assumes a standard Ubuntu, tested on 18.04, 20.04, and latest docker. (When
         g++ clang make cmake curl git wget \
         liblapack-dev libf2c2-dev libqhull-dev libeigen3-dev libann-dev libccd-dev \
         libjsoncpp-dev libyaml-cpp-dev libpoco-dev libboost-system-dev portaudio19-dev libusb-1.0-0-dev \
-        libx11-dev libglu1-mesa-dev libglfw3-dev libglew-dev freeglut3-dev libpng-dev libassimp-dev \
+        libx11-dev libglu1-mesa-dev libglfw3-dev libglew-dev freeglut3-dev libpng-dev libassimp-dev
       mkdir -p $HOME/git $HOME/.local
 
 * External libraries: You can skip librealsense and libfranka if you disable below. To standardize installations, I use a [basic install script](https://github.com/MarcToussaint/rai-extern/blob/main/install.sh) (have a look, if you have concerns what it does).
@@ -49,19 +49,19 @@ This assumes a standard Ubuntu, tested on 18.04, 20.04, and latest docker. (When
 * You can skip this, if you disable pybind11 below.
 
       sudo apt install --yes python3-dev python3 python3-pip
-      echo 'export PATH="${PATH}:$HOME/.local/bin"' >> ~/.bashrc   #add this to your .bashrc, if not done already
-      echo 'export PYTHONPATH="${PYTHONPATH}:$HOME/.local/lib"' >> ~/.bashrc   #add this to your .bashrc, if not done already
       python3 -m pip install --user numpy pybind11 pybind11-stubgen
-
+	  
+	  # add the following to your .bashrc, if not done already
+      echo 'export PATH="${PATH}:$HOME/.local/bin"' >> ~/.bashrc
+      echo 'export PYTHONPATH="${PYTHONPATH}:$HOME/.local/lib"' >> ~/.bashrc
 
 * Finally, clone and compile this repo:
 
       cd $HOME/git
 	  git clone --recurse-submodules [email protected]:MarcToussaint/botop.git
       cd botop
-	  env APTGETYES=1 make -C rai -j1 installUbuntuAll  # calls sudo apt-get install
       export PY_VERSION=`python3 -c "import sys; print(str(sys.version_info[0])+'.'+str(sys.version_info[1]))"`
-      cmake -DUSE_BULLET=OFF -DPY_VERSION=$PY_VERSION . -B build  #options: disable USE_LIBFRANKA USE_REALSENSE USE_PYBIND USE_PHYSX
+      cmake -DUSE_BULLET=OFF -DUSE_OPENCV=OFF -DPY_VERSION=$PY_VERSION . -B build  #options: disable USE_LIBFRANKA USE_REALSENSE USE_PYBIND USE_PHYSX
 	  make -C build
 
 * Optionally install lib, headers, and binaries to ~/.local

src/MarkerVision/cvTools.cpp

@@ -14,11 +14,16 @@ void decomposeInvProjectionMatrix(arr& K, arr& R, arr& t, const arr& P){
   lapack_RQ(K, R, KRt);
   //cout <<"K*R" <<K*R <<"\nKR:" <<KRt <<endl;
   transpose(R);
-  if(K(2,2)<0.){ //transforms the solution to be consistent with convention $K(2,2)\approx 1$
+  {
+    //transforms the solution to be consistent with convention $K(2,2)\approx 1$
     arr I=eye(3);
-    I(1,1) = I(2,2) = -1.;
-    R = R*I;
-    K = K*I;
+    uint num=0;
+    for(uint i=0;i<3;i++) if(K(i,i)<0.){ I(i,i) = -1.; num++; }
+    if(num){
+      if(num!=2) LOG(-1) <<"??? is your cam left handed ???";
+      R = R*I;
+      K = K*I;
+    }
   }
 }
 

test/14-calibPoints/main.cpp

@@ -45,7 +45,7 @@ void collectData(){
 
   arr dots = rai::getParameter<arr>("dots");
   uint views = rai::getParameter<uint>("views");
-  dots.reshape(-1,3);
+  dots.reshape(-1, 2);
 
   OpenGL imgGl;
 
@@ -75,7 +75,7 @@ void collectData(){
       rai::Frame* dot = C[STRING("dot" <<d)];
 
       arr dotPos = arr{.2, .15, .2}%(rand(3)-.5);
-      dotPos(2) += .35;
+      dotPos(2) += .3;
       double wristAngle = rnd.uni(-2.3, 2.3);
 
       {
@@ -187,7 +187,7 @@ void computeCalibration(){
       <<"\nblob-from-hom: " <<x
      <<"\ncalib err:     " <<x-xCam <<endl;
 
-    disp.watchImage(img, checks>0, 1.);
+    disp.watchImage(img, checks>1, 1.);
 
     //collect data
     U.append(uHom);
@@ -301,15 +301,16 @@ void komoCalibrate(){
     byteA img(dat.get<byteA>("img"));
     floatA depth(dat.get<floatA>("depth"));
     arr fxycxy = dat.get<arr>("camFxycxy");
-    arr u = getHsvBlobImageCoords(img, depth, hsvFilter, 0, histogram);
+    arr u = getHsvBlobImageCoords(img, depth, hsvFilter, checks, histogram);
+    if(!u.N) continue;
     arr xCam = u;
     depthData2point(xCam, fxycxy); //transforms the point to camera xyz coordinates
     rai::Frame *viewP = komo.timeSlices(t, viewPoint->ID);
     viewP->setRelativePosition(xCam);
 
     //    cout <<t <<' ' <<q <<viewP->name <<viewP->getPosition() <<endl;
 
-    n->key.resize(n->key.N-1, true);
+    n->key.resize(4, true);
 
     //add calibration objective
     komo.addObjective({double(t+1)}, FS_positionDiff, {"viewPoint", n->key}, OT_sos, {1e2});
@@ -337,7 +338,7 @@ void komoCalibrate(){
   sol.opt.set_stopTolerance(1e-6);
   sol.opt.set_verbose(0);
   auto ret = sol.solve();
-//  cout <<komo.report(false) <<endl;
+  cout <<komo.report(false) <<endl; //reports match per feature..
   cout <<"-- result: " <<*ret <<endl;
   cout <<"== optimized parameters (camera, dots): " <<ret->x <<endl;
 
@@ -353,9 +354,9 @@ void komoCalibrate(){
 
   komo.view(true, "after optim");
 
-//  FILE("z.hsv") <<(~histogram).modRaw() <<endl;
-//  gnuplot("set title 'HSV histogram'; plot 'z.hsv' us 0:1 t 'H', '' us 0:2 t 'S', '' us 0:3 t 'V'");
-//  rai::wait();
+  FILE("z.hsv") <<(~histogram).modRaw() <<endl;
+  gnuplot("set title 'HSV histogram'; plot 'z.hsv' us 0:1 t 'H', '' us 0:2 t 'S', '' us 0:3 t 'V'");
+  rai::wait();
 }
 
 //===========================================================================
@@ -380,6 +381,7 @@ void demoCalibration(){
   int checks = rai::getParameter<int>("checks", 1);
 
   BotOp bot(C, rai::getParameter<bool>("real", false));
+  bot.home(C);
 
   for(uint i=0;;i++){
     rai::Frame *dot = C.getFrame(STRING("dot"<<i), false);
@@ -394,13 +396,15 @@ void demoCalibration(){
 
     //fine motion
     bot.gripperMove(rai::_left, 0);
-//    for(uint t=0;t<5;t++) bot.sync(C);
-//    if(!sense_HsvBlob(bot, C, cam->name, "target", hsvFilter, Pinv, checks)) return;
+#if 1
+    for(uint t=0;t<5;t++) bot.sync(C);
+    if(!sense_HsvBlob(bot, C, cam->name, "target", hsvFilter, Pinv, checks)) return;
 //    target->setRelativePosition(dot->getRelativePosition());
+#endif
     {
       Move_IK move(bot, C, checks);
       move().addObjective({}, FS_vectorZ, {"l_gripper"}, OT_eq, {1e0}, {0.,0.,1.});
-      move().addObjective({}, FS_positionDiff, {"l_gripper", dot->name}, OT_eq, {1e0}, {0.,0., .01});
+      move().addObjective({}, FS_positionDiff, {"l_gripper", "target" /*dot->name*/}, OT_eq, {1e0}, {0.,0., .01});
       if(!move.go()) return;
     }
 
@@ -445,12 +449,12 @@ int main(int argc, char * argv[]){
 
 //  collectData();
 
-//  computeCalibration();
+  computeCalibration();
 //  komoCalibrate();
 
 //  selectHSV();
 
-  demoCalibration();
+//  demoCalibration();
 
 //  checkTip();
 

test/14-calibPoints/rai.cfg

@@ -1,4 +1,4 @@
-#real
+real
 checks: 1
 
 #bot/useOptitrack: true
@@ -11,17 +11,18 @@ botsim/verbose: 0
 #RealSense/exposure: 100
 #RealSense/white: 2500
 
+dots: [0.180749, 0.101301, -0.295883, 0.0993051, -0.460925, -0.151945, -0.451812, -0.426068]
 dots: [0.180436, 0.101313, -0.294048, 0.10003, -0.459017, -0.15235, -0.452994, -0.425162]
 dots: [
  0.176193, 0.104552,
 -0.300173, 0.108897,
 -0.45, -0.1,
 -0.45, -0.5,
 ]
-views: 10
+views: 20
 
 #real day
-hsvFilter: [95, 100, 80,
+hsvFilter: [95, 200, 80,
  110, 255, 200]
 
 #real in evening
@@ -30,11 +31,17 @@ hsvFilter: [77, 105, 66,
 
 
 #QcameraWrist: [0.0612469, -0.0228876, 0.156111, 0.36824, -0.0141277, 0.026341, -0.910506]
+QcameraWrist: [0.0566288, -0.0138618, 0.158583, 0.371288, -0.0124238, 0.0272688, -0.928034]
 QcameraWrist: [0.0569686, -0.0139042, 0.15845, 0.372332, -0.0123119, 0.0269895, -0.927625]
+[0.058572, -0.0142142, 0.160157, 0.927853, 0.0270032, 0.0137342, 0.371713]
 
 #real
 Pinv: []
 
+Pinv:[-0.00231553, 0.00218067, 0.387915, 0.058572,
+ -0.00221068, -0.00231521, 1.08355, -0.0142142,
+ 1.7325e-05, -0.000192083, 1.02193, 0.160157]
+
 Pinv: [0.00315592, 0.000106899, -1.02654, -0.00219142,
 3.80978e-05, -0.0031546, 0.550581, -0.00549389,
 -1.35119e-06, 2.42059e-05, -0.999362, -0.00735351]