CV学习日志：CV开发之便携仿真器
Posted dzybk
tags:
篇首语：本文由小常识网(cha138.com)小编为大家整理，主要介绍了CV学习日志：CV开发之便携仿真器相关的知识，希望对你有一定的参考价值。
视觉算法开发过程中，如何快速地验证算法是一个头疼的问题。
通常有两种方式，或是下载公开数据来验证，或是通过仿真环境采集数据来验证，然而这两种方式都不便携，即都需要大量的准备工作和一定的存储要求。
本文介绍一个简单袖珍的仿真器，可快速获取视觉算法所需要的验证数据，用于视觉标定和视觉定位等算法。
此仿真器主要是由一系列世界坐标点和一个运动相机组成，运动相机从世界坐标系的原点运动到不能再观察到足够多的世界坐标点时或已进行足够次数的观察后退出，每运动一个步长进行一次观察，运动信息参见后文或源码，观察时获取哪些数据参见源码。
关于仿真器的大致信息如下：
(1)关于坐标系：世界系采用Z轴向下的右手坐标系，相机系亦同。
(2)关于世界目标：在世界中创建了100*100个世界坐标，所有Z坐标是-1~-99的随机分布，设I从0到100且J从0到100，X坐标的变化规则是10乘以J再加一个0~9的随机数，Y坐标的变化规则是10乘以I再加一个0~9的随机数。
(3)关于观察位置变化：相机每次成像时的Z坐标是-101~-150的一个随机数，X坐标等于上一次的X坐标加一个0~9的随机数，Y坐标的变化规律也一样，初始X和Y都为0。也可设置观察位置无变化，即相机始终位于初次观察位置。
(4)关于观察姿态变化：相机每次成像时的Z朝向是-5~5的一个随机角度，Y朝向和X朝向变化规律也一样。也可设置观察朝向始终为0或只有XYZ中的两个朝向或只有XYZ中的一个朝向。
(5)关于运动退出条件：理论上只图像足够大，100*100个世界坐标对应100*100个像素坐标，但若限制图像尺寸后则每次成像时都有部分像素坐标不可用，这里限制当可用的像素坐标少于指定值(默认128)时则相机退出运动。退出的另一条件是已进行指定次数(默认INT_MAX)的观察。
(6)关于可视化：启动可视化后，按空格可逐次可视化每次观察，当到达最后一次观察后则返回第一次观察。
以下是详细代码，依赖于C++14、OpenCV4.x和Spdlog。
1 #include <opencv2/opencv.hpp> 2 #include <opencv2/viz.hpp> 3 #include <ceres/ceres.h> 4 #include <spdlog/spdlog.h> 5 using namespace std; 6 using namespace cv; 7 8 class MotionSim 9 { 10 public: 11 static void TestMe(int argc, char** argv) 12 { 13 MotionSim motionSim; 14 motionSim.visMotion(); 15 } 16 17 public: 18 struct MotionNode 19 { 20 Mat_<double> r = Mat_<double>(3, 1); 21 Mat_<double> t = Mat_<double>(3, 1); 22 Mat_<Vec3d> point3D; 23 Mat_<Vec2d> point2D; 24 Mat_<int> point3DIds; 25 Mat_<double> K; 26 Mat_<double> D; 27 Mat_<double> R = Mat_<double>(3, 3); 28 Mat_<double> T = Mat_<double>(3, 4); 29 Mat_<double> degree = Mat_<double>(3, 1); 30 string print(string savePath = "") 31 { 32 string str; 33 str += fmt::format("K: {} ", cvarr2str(K)); 34 str += fmt::format("D: {} ", cvarr2str(D)); 35 str += fmt::format("r: {} ", cvarr2str(r)); 36 str += fmt::format("t: {} ", cvarr2str(t)); 37 str += fmt::format("R: {} ", cvarr2str(R)); 38 str += fmt::format("T: {} ", cvarr2str(T)); 39 str += fmt::format("degree: {} ", cvarr2str(degree)); 40 if (savePath.empty() == false) { FILE* out = fopen(savePath.c_str(), "w"); fprintf(out, str.c_str()); fclose(out); } 41 return str; 42 } 43 }; 44 static string cvarr2str(InputArray v) 45 { 46 Ptr<Formatted> fmtd = cv::format(v, Formatter::FMT_DEFAULT); 47 string dst; fmtd->reset(); 48 for (const char* str = fmtd->next(); str; str = fmtd->next()) dst += string(str); 49 return dst; 50 } 51 static void euler2matrix(double degree[3], double matrix[9]) 52 { 53 double deg2rad = 3.14159265358979323846 * 0.0055555555555555556; 54 double radian[3] = { degree[0] * deg2rad,degree[1] * deg2rad, degree[2] * deg2rad }; 55 double sinR = sin(radian[0]); 56 double sinP = sin(radian[1]); 57 double sinY = sin(radian[2]); 58 double cosR = cos(radian[0]); 59 double cosP = cos(radian[1]); 60 double cosY = cos(radian[2]); 61 62 //RPY indicates: first Yaw aroundZ, second Pitch aroundY, third Roll aroundX 63 matrix[0] = cosY * cosP; matrix[1] = cosY * sinP * sinR - sinY * cosR; matrix[2] = cosY * sinP * cosR + sinY * sinR; 64 matrix[3] = sinY * cosP; matrix[4] = sinY * sinP * sinR + cosY * cosR; matrix[5] = sinY * sinP * cosR - cosY * sinR; 65 matrix[6] = -sinP; matrix[7] = cosP * sinR; matrix[8] = cosP * cosR; 66 }; 67 68 private: 69 const int nHorPoint3D = 100; 70 const int nVerPoint3D = 100; 71 const double varPoint3DXY = 10.; 72 const double minPoint3DZ = 1.; 73 const double maxPoint3DZ = 99.; 74 const double minCamZ = 101.; 75 const double maxCamZ = 150.; 76 const double varCamDegree = 5.; 77 Mat_<Vec3d> allPoint3D = Mat_<Vec3d>(nVerPoint3D * nHorPoint3D, 1); 78 Mat_<double> allPoint3DZ = Mat_<double>(nVerPoint3D * nHorPoint3D, 1); 79 Mat_<double> K = Mat_<double>(3, 3, 0.); 80 Mat_<double> D = Mat_<double>(8, 1, 0.); 81 82 public: 83 vector<MotionNode> motionNodes;//World Information: RightX, FrontY, DownZ 84 MotionSim(int imaSide = 512, int leastNodeTheRestart = 32, int mostNodeThenExit = INT_MAX, int leastFeaturesThenExit = 128, 85 int rotAxis = 1 + 2 + 4, bool isTrans = true, bool enableRandomSeek = true, bool enableVerbose = false) 86 { 87 if (enableRandomSeek) cv::setRNGSeed(clock()); 88 while (motionNodes.size() < leastNodeTheRestart) 89 { 90 //1.GetAllPoint3D 91 cv::randu(allPoint3DZ, -maxPoint3DZ, -minPoint3DZ);//DownZ 92 for (int i = 0, k = 0; i < nVerPoint3D; ++i) 93 for (int j = 0; j < nHorPoint3D; ++j, ++k) 94 allPoint3D(k) = Vec3d((j + cv::randu<double>()) * varPoint3DXY, (i + cv::randu<double>()) * varPoint3DXY, allPoint3DZ(i, j)); 95 96 //2.GetCamParams 97 cv::randu(K, imaSide / 2 - 10., imaSide / 2 + 10.); K(0, 1) = K(1, 0) = K(2, 0) = K(2, 1) = 0; K(2, 2) = 1; 98 cv::randu(D, -1.0, 1.0); 99 100 //3.GetAllMotionNode 101 motionNodes.clear(); 102 for (int64 k = 0; ; ++k) 103 { 104 //3.1 JoinCamParams 105 MotionNode node; 106 node.K = K.clone(); 107 node.D = D.clone(); 108 109 //3.2 GetCamTrans 110 if (k == 0) node.t(0) = node.t(1) = 0; 111 else 112 { 113 node.t(0) = motionNodes[k - 1].t(0) + cv::randu<double>() * varPoint3DXY; 114 node.t(1) = motionNodes[k - 1].t(1) + cv::randu<double>() * varPoint3DXY; 115 } 116 node.t(2) = minCamZ + cv::randu<double>() * (maxCamZ - minCamZ); 117 node.t(2) = -node.t(2);//DownZ 118 if (isTrans == false && k != 0) { node.t(0) = motionNodes[0].t(0); node.t(1) = motionNodes[0].t(1); node.t(2) = motionNodes[0].t(2); } 119 120 //3.3 GetCamRot 121 node.degree = 0.; 122 if (rotAxis & 1) node.degree(0) = cv::randu<double>() * varCamDegree; 123 if (rotAxis & 2) node.degree(1) = cv::randu<double>() * varCamDegree; 124 if (rotAxis & 4) node.degree(2) = cv::randu<double>() * varCamDegree; 125 this->euler2matrix(node.degree.ptr<double>(), node.R.ptr<double>()); 126 cv::Rodrigues(node.R, node.r); 127 cv::hconcat(node.R, node.t, node.T); 128 129 //3.4 GetPoint3DAndPoint2D 130 Mat_<Vec2d> allPoint2D; 131 cv::projectPoints(allPoint3D, -node.r, -node.R.t() * node.t, node.K, node.D, allPoint2D); 132 for (int k = 0; k < allPoint2D.total(); ++k) 133 if (allPoint2D(k)[0] > 0 && allPoint2D(k)[0] < imaSide && allPoint2D(k)[1] > 0 && allPoint2D(k)[1] < imaSide) 134 { 135 node.point2D.push_back(allPoint2D(k)); 136 node.point3D.push_back(allPoint3D(k)); 137 node.point3DIds.push_back(k); 138 } 139 140 //3.5 PrintDetails 141 motionNodes.push_back(node); 142 if (enableVerbose) 143 { 144 cout << endl << node.print(); 145 cout << fmt::format("view={} features={} ", k, node.point2D.rows); 146 double minV = 0, maxV = 0;//Distortion makes some minV next to maxV 147 int minId = 0, maxId = 0; 148 cv::minMaxIdx(allPoint2D.reshape(1, int(allPoint2D.total()) * allPoint2D.channels()), &minV, &maxV, &minId, &maxId); 149 cout << fmt::format("minInfo:({}, {})", minId, minV) << allPoint3D(minId / 2) << allPoint2D(minId / 2) << endl; 150 cout << fmt::format("maxInfo:({}, {})", maxId, maxV) << allPoint3D(maxId / 2) << allPoint2D(maxId / 2) << endl; 151 std::getchar(); 152 } 153 if (node.point2D.rows < leastFeaturesThenExit || motionNodes.size() > mostNodeThenExit) break; 154 } 155 } 156 } 157 void visMotion() 158 { 159 //1.CreateWidgets 160 Size2d validSize(nHorPoint3D * varPoint3DXY, nVerPoint3D * varPoint3DXY); 161 Mat_<cv::Affine3d> camPoses(int(motionNodes.size()), 1); for (int k = 0; k < camPoses.rows; ++k) camPoses(k) = cv::Affine3d(motionNodes[k].T); 162 viz::WText worldInfo(fmt::format("nMotionNode: {} K: {} D: {}", motionNodes.size(), cvarr2str(K), cvarr2str(D)), Point(10, 240), 10); 163 viz::WCoordinateSystem worldCSys(1000); 164 viz::WPlane worldGround(Point3d(validSize.width / 2, validSize.height / 2, 0), Vec3d(0, 0, 1), Vec3d(0, 1, 0), validSize); 165 viz::WCloud worldPoints(allPoint3D, Mat_<Vec3b>(allPoint3D.size(), Vec3b(0, 255, 0))); 166 viz::WTrajectory camTraj1(camPoses, viz::WTrajectory::FRAMES, 8); 167 viz::WTrajectorySpheres camTraj2(camPoses, 100, 2); 168 viz::WTrajectoryFrustums camTraj3(camPoses, Matx33d(K), 4., viz::Color::yellow()); 169 worldCSys.setRenderingProperty(viz::OPACITY, 0.1); 170 worldGround.setRenderingProperty(viz::OPACITY, 0.1); 171 camTraj2.setRenderingProperty(viz::OPACITY, 0.6); 172 173 //2.ShowWidgets 174 static viz::Viz3d viz3d(__FUNCTION__); 175 viz3d.showWidget("worldInfo", worldInfo); 176 viz3d.showWidget("worldCSys", worldCSys); 177 viz3d.showWidget("worldGround", worldGround); 178 viz3d.showWidget("worldPoints", worldPoints); 179 viz3d.showWidget("camTraj1", camTraj1); 180 viz3d.showWidget("camTraj2", camTraj2); 181 viz3d.showWidget("camTraj3", camTraj3); 182 183 //3.UpdateWidghts 184 static const vector<MotionNode>& nodes = motionNodes; 185 viz3d.registerKeyboardCallback([](const viz::KeyboardEvent& keyboarEvent, void* pVizBorad)->void 186 { 187 if (keyboarEvent.action != viz::KeyboardEvent::KEY_DOWN) return; 188 static int ind = 0; 189 if (keyboarEvent.code == ‘ ‘) 190 { 191 size_t num = nodes.size(); 192 if (ind != 0) 193 { 194 for (int k = 0; k < nodes[ind % num == 0 ? num - 1 : ind % num - 1].point3D.rows; ++k) viz3d.removeWidget("active" + std::to_string(k)); 195 viz3d.removeWidget("nodeInfo"); 196 viz3d.removeWidget("camSolid"); 197 } 198 for (int k = 0; k < nodes[ind % num].point3D.rows; ++k) viz3d.showWidget("active" + std::to_string(k), viz::WSphere(nodes[ind % num].point3D(k), 5, 10)); 199 viz3d.showWidget("nodeInfo", viz::WText(fmt::format("CurrentNode: {} ValidPoit3D: {}", ind % num, nodes[ind % num].point3D.rows), Point(10, 10), 10)); 200 viz3d.showWidget("camSolid", viz::WCameraPosition(Matx33d(nodes[ind % num].K), 10, viz::Color::yellow()), cv::Affine3d(nodes[ind % num].T)); 201 ++ind; 202 } 203 }, 0); 204 viz3d.spin(); 205 } 206 }; 207 208 class OptimizeRt : public ceres::CostFunction 209 { 210 public://Test 211 using MotionNode = MotionSim::MotionNode; 212 static void TestMe(int argc, char** argv) 213 { 214 MotionSim motionSim; 215 for (size_t k = 0; k < motionSim.motionNodes.size(); ++k) 216 { 217 218 } 219 } 220 221 public://Core 222 const MotionNode& motionNode; 223 OptimizeRt(MotionNode motionNode0) : motionNode(motionNode0) 224 { 225 this->set_num_residuals(motionNode.point3D.rows * 2); 226 this->mutable_parameter_block_sizes()->push_back(6);//also could use two groups of params: r and t 227 } 228 bool Evaluate(double const* const* params, double* residuals, double** jacobians) const 229 { 230 //1.FormatInput 231 Matx31d r(params[0]); 232 Matx31d t(params[0] + 3); 233 Mat_<Vec2d> errVector(motionNode.point2D.rows, 1, (Vec2d*)(residuals)); 234 Mat_<double> dpdrt; 235 if (jacobians && jacobians[0]) dpdrt = Mat_<double>(motionNode.point2D.rows * 2, 6, jacobians[0]); 236 237 ////2.CalcErrorAndJacobian 238 //Mat_<double> dpdKDT; 239 //Mat_<Matx21d> pDs2(pDs.size()); 240 //cv::projectPoints(PWs, r, t, K, D, pDs2, dpdKDT); 241 //errVector = pDs2 - pDs; 242 //if (jacobians && jacobians[0]) dpdKDT.colRange(0, 6).copyTo(dpdrt); 243 244 return true; 245 } 246 }; 247 248 int main(int argc, char** argv) { MotionSim::TestMe(argc, argv); return 0; }
以上是关于CV学习日志：CV开发之便携仿真器的主要内容，如果未能解决你的问题，请参考以下文章
巧用Vscode编辑器，快速编辑代码，教你一键写完一段代码，向合格的cv工程师前进
巧用Vscode编辑器，快速编辑代码，教你一键写完一段代码，向合格的cv工程师前进
巧用Vscode编辑器，快速编辑代码，教你一键写完一段代码，向合格的cv工程师前进
巧用Vscode编辑器，快速编辑代码，教你一键写完一段代码，向合格的cv工程师前进