CV学习日志：CV开发之便携仿真器

  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: {}\n", cvarr2str(K));
 34             str += fmt::format("D: {}\n", cvarr2str(D));
 35             str += fmt::format("r: {}\n", cvarr2str(r));
 36             str += fmt::format("t: {}\n", cvarr2str(t));
 37             str += fmt::format("R: {}\n", cvarr2str(R));
 38             str += fmt::format("T: {}\n", cvarr2str(T));
 39             str += fmt::format("degree: {}\n", 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={}\n", 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: {}\nK: {}\nD: {}", 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: {}\nValidPoit3D: {}", 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; }