int problem3_main(){ std::ofstream fout1("problem3.30.txt"); solve(30.0 / 180.0 * acos(-1), fout1); fout1.close(); std::ofstream fout2("problem3.40.txt"); solve(40.0 / 180.0 * acos(-1), fout2); fout2.close(); std::ofstream fout3("problem3.50.txt"); solve(50.0 / 180.0 * acos(-1), fout3); fout3.close(); B2_M = 0.0; std::ofstream fout4("problem3-no-air.30.txt"); solve(30.0 / 180.0 * acos(-1), fout4); fout4.close(); std::ofstream fout5("problem3-no-air.40.txt"); solve(40.0 / 180.0 * acos(-1), fout5); fout5.close(); std::ofstream fout6("problem3-no-air.50.txt"); solve(50.0 / 180.0 * acos(-1), fout6); fout6.close(); return 0; }
int main() { std::ofstream fout1("test1.dat"); // Flat Sky std::ofstream fout2("test2.dat"); // Spherical near equator std::ofstream fout3("test3.dat"); // Spherical around N pole. std::ofstream fout4("test4.dat"); // Test1 with random offsets std::ofstream fout4c("test4_centers.dat"); // Corresponding centers for test4.dat fout1.precision(12); fout2.precision(12); fout3.precision(12); fout4.precision(12); fout4c.precision(12); double ra0 = 0.; // RA, Dec of center for test2. double dec0 = 80.; const long ngal = 100000; const double rscale = 1.; // Scale radius in degrees. const double rmax = 5.; // In units of rscale // The functional form imprinted is purely radial: // gamma_t(r) = gamma0/r // kappa(r) = kappa0/r // n(r) ~ exp(-r^2/2) // where r is in units of rscale const double gamma0 = 1.e-3; const double kappa0 = 3.e-3; // Make sure gamma_t < 0.5 const double rmin = gamma0/0.5; srand(1234); // To make it deterministic. // Normally for the Box-Muller Transformation, one would restrict rsq < 1 // But we then want to clip the resulting distribution at rmax, so // rsq * fac^2 < rmax^2 // rsq * (-2.*log(rsq)/rsq) < rmax^2 // -2. * log(rsq) < rmax^2 // rsq > exp(-rmax^2/2) const double rsqmin = exp(-rmax*rmax/2.); const double rsqmax = exp(-rmin*rmin/2.); //std::cout<<"rsq min/max = "<<rsqmin<<','<<rsqmax<<std::endl; double xdeg_min=0., xdeg_max=0., var_xdeg=0.; double ydeg_min=0., ydeg_max=0., var_ydeg=0.; for (long n=0; n<ngal; ++n) { double x,y,rsq; do { x = double(rand()) / RAND_MAX; // Random number from 0..1 y = double(rand()) / RAND_MAX; //std::cout<<"x,y = "<<x<<','<<y<<std::endl; x = 2.*x-1.; // Now from -1..1 y = 2.*y-1.; //std::cout<<"x,y => "<<x<<','<<y<<std::endl; rsq = x*x+y*y; //std::cout<<"rsq = "<<rsq<<std::endl; } while (rsq <= rsqmin || rsq >= rsqmax); // Use Box-Muller Transformation to convert to Gaussian distribution in x,y. double fac = sqrt(-2.*log(rsq)/rsq); x *= fac; y *= fac; double r = fac*sqrt(rsq); double theta = atan2(y,x); double g = gamma0 / r; double k = kappa0 / r; assert(g < 0.5); double xdeg = x*rscale; double ydeg = y*rscale; double rdeg = r*rscale; // Do some sanity checks: if (xdeg < xdeg_min) xdeg_min = xdeg; if (xdeg > xdeg_max) xdeg_max = xdeg; if (ydeg < ydeg_min) ydeg_min = ydeg; if (ydeg > ydeg_max) ydeg_max = ydeg; var_xdeg += xdeg*xdeg; var_ydeg += ydeg*ydeg; // // Flat sky: // double g1 = -g * cos(2.*theta); double g2 = -g * sin(2.*theta); fout1 << xdeg <<" "<< ydeg <<" "<< g1 <<" "<< g2 <<" "<< k <<std::endl; // With offsets in position: double dx = 2.*double(rand()) / RAND_MAX - 1.; // Random number from -1..1 double dy = 2.*double(rand()) / RAND_MAX - 1.; dx *= rscale; dy *= rscale; fout4 << xdeg + dx <<" "<< ydeg + dy <<" "<< g1 <<" "<< g2 <<" "<< k <<std::endl; fout4c << dx <<" "<< dy <<" 1. "<<std::endl; // // Spherical near equator: // // Use spherical triangle with A = point, B = (ra0,dec0), C = N. pole // a = Pi/2-dec0 // c = 2*atan(r/2) // B = Pi/2 - theta // Solve the rest of the triangle with spherical trig: double c = 2.*atan( (rdeg*M_PI/180.) / 2.); double a = M_PI/2. - (dec0*M_PI/180.); double B = x > 0 ? M_PI/2. - theta : theta - M_PI/2.; if (B < 0) B += 2.*M_PI; if (B > 2.*M_PI) B -= 2.*M_PI; double cosb = cos(a)*cos(c) + sin(a)*sin(c)*cos(B); double b = std::abs(cosb) < 1. ? acos(cosb) : 0.; double cosA = (cos(a) - cos(b)*cos(c)) / (sin(b)*sin(c)); double A = std::abs(cosA) < 1. ? acos(cosA) : 0.; double cosC = (cos(c) - cos(a)*cos(b)) / (sin(a)*sin(b)); double C = std::abs(cosC) < 1. ? acos(cosC) : 0.; //std::cout<<"x,y = "<<x<<','<<y<<std::endl; //std::cout<<"a = "<<a<<std::endl; //std::cout<<"b = "<<b<<std::endl; //std::cout<<"c = "<<c<<std::endl; //std::cout<<"A = "<<A<<std::endl; //std::cout<<"B = "<<B<<std::endl; //std::cout<<"C = "<<C<<std::endl; // Compute ra,dec from these. // Note: increasing x is decreasing ra. East is left on the sky! double ra = x>0 ? -C : C; double dec = M_PI/2. - b; ra *= 180. / M_PI; dec *= 180. / M_PI; ra += ra0; //std::cout<<"ra = "<<ra<<std::endl; //std::cout<<"dec = "<<dec<<std::endl; // Rotate shear relative to local west std::complex<double> gamma(g1,g2); double beta = M_PI - (A+B); if (x > 0) beta = -beta; //std::cout<<"gamma = "<<gamma<<std::endl; //std::cout<<"beta = "<<beta<<std::endl; std::complex<double> exp2ibeta(cos(2.*beta),sin(2.*beta)); gamma *= exp2ibeta; //std::cout<<"gamma => "<<gamma<<std::endl; fout2 << ra <<" "<< dec <<" "<< real(gamma) <<" "<<imag(gamma) <<" "<<k <<std::endl; // // Spherical around N pole // dec = 90. - c * 180./M_PI; ra = theta * 12. / M_PI; fout3 << ra <<" "<< dec <<" "<< g <<" "<<0. <<" "<<k <<std::endl; } var_xdeg /= ngal; var_ydeg /= ngal; std::cout<<"Min/Max x = "<<xdeg_min<<" "<<xdeg_max<<std::endl;; std::cout<<"Min/Max y = "<<ydeg_min<<" "<<ydeg_max<<std::endl;; std::cout<<"sqrt(Var(x)) = "<<sqrt(var_xdeg)<<std::endl; std::cout<<"sqrt(Var(y)) = "<<sqrt(var_ydeg)<<std::endl; // Make random catalogs std::ofstream foutr1("rand1.dat"); std::ofstream foutr2("rand2.dat"); std::ofstream foutr3("rand3.dat"); foutr1.precision(12); foutr2.precision(12); foutr3.precision(12); xdeg_min=xdeg_max=var_xdeg=0.; ydeg_min=ydeg_max=var_ydeg=0.; for (long n=0; n<10*ngal; ++n) { double x,y,rsq; do { x = double(rand()) / RAND_MAX; // Random number from 0..1 y = double(rand()) / RAND_MAX; x = 2.*x-1.; // Now from -1..1 y = 2.*y-1.; rsq = x*x+y*y; } while (rsq >= 1.); x *= rmax; y *= rmax; double r = rmax*sqrt(rsq); double theta = atan2(y,x); double xdeg = x*rscale; double ydeg = y*rscale; double rdeg = r*rscale; // Do some sanity checks: if (xdeg < xdeg_min) xdeg_min = xdeg; if (xdeg > xdeg_max) xdeg_max = xdeg; if (ydeg < ydeg_min) ydeg_min = ydeg; if (ydeg > ydeg_max) ydeg_max = ydeg; var_xdeg += xdeg*xdeg; var_ydeg += ydeg*ydeg; // // flat sky: // foutr1 << xdeg <<" "<< ydeg << std::endl; // // Spherical near equator: // double c = 2.*atan( (rdeg*M_PI/180.) / 2.); double a = M_PI/2. - (dec0*M_PI/180.); double B = x > 0 ? M_PI/2. - theta : theta - M_PI/2.; if (B < 0) B += 2.*M_PI; if (B > 2.*M_PI) B -= 2.*M_PI; double cosb = cos(a)*cos(c) + sin(a)*sin(c)*cos(B); double b = std::abs(cosb) < 1. ? acos(cosb) : 0.; double cosC = (cos(c) - cos(a)*cos(b)) / (sin(a)*sin(b)); double C = std::abs(cosC) < 1. ? acos(cosC) : 0.; double ra = x>0 ? -C : C; double dec = M_PI/2. - b; ra *= 180. / M_PI; dec *= 180. / M_PI; ra += ra0; foutr2 << ra <<" "<< dec <<std::endl; // // Spherical around N pole // dec = 90. - c * 180./M_PI; ra = theta * 12. / M_PI; foutr3 << ra <<" "<< dec <<std::endl; } var_xdeg /= ngal; var_ydeg /= ngal; std::cout<<"For randoms:\n"; std::cout<<"Min/Max x = "<<xdeg_min<<" "<<xdeg_max<<std::endl;; std::cout<<"Min/Max y = "<<ydeg_min<<" "<<ydeg_max<<std::endl;; std::cout<<"sqrt(Var(x)) = "<<sqrt(var_xdeg)<<std::endl; std::cout<<"sqrt(Var(y)) = "<<sqrt(var_ydeg)<<std::endl; return 0; }
int main(int argc, char * argv[]) { int width = 640; int height = 480; Resolution::getInstance(width, height); Intrinsics::getInstance(528, 528, 320, 240); cv::Mat intrinsicMatrix = cv::Mat(3,3,CV_64F); intrinsicMatrix.at<double>(0,0) = Intrinsics::getInstance().fx(); intrinsicMatrix.at<double>(1,1) = Intrinsics::getInstance().fy(); intrinsicMatrix.at<double>(0,2) = Intrinsics::getInstance().cx(); intrinsicMatrix.at<double>(1,2) = Intrinsics::getInstance().cy(); intrinsicMatrix.at<double>(0,1) =0; intrinsicMatrix.at<double>(1,0) =0; intrinsicMatrix.at<double>(2,0) =0; intrinsicMatrix.at<double>(2,1) =0; intrinsicMatrix.at<double>(2,2) =1; Bytef * decompressionBuffer = new Bytef[Resolution::getInstance().numPixels() * 2]; IplImage * deCompImage = 0; std::string logFile="/home/lili/Kinect_Logs/2015-11-05.00.klg"; // assert(pcl::console::parse_argument(argc, argv, "-l", logFile) > 0 && "Please provide a log file"); RawLogReader logReader(decompressionBuffer, deCompImage, logFile, true); cv::Mat1b tmp(height, width); cv::Mat3b depthImg(height, width); PlaceRecognition placeRecognition(&intrinsicMatrix); iSAMInterface iSAM; //Keyframes KeyframeMap map(true); Eigen::Vector3f lastPlaceRecognitionTrans = Eigen::Vector3f::Zero(); Eigen::Matrix3f lastPlaceRecognitionRot = Eigen::Matrix3f::Identity(); int64_t lastTime = 0; OdometryProvider * odom = 0; //int frame_index = 0; // uint64_t timestamp; /*if(true) { odom = new FOVISOdometry; if(logReader.hasMore()) { logReader.getNext(); Eigen::Matrix3f Rcurr = Eigen::Matrix3f::Identity(); Eigen::Vector3f tcurr = Eigen::Vector3f::Zero(); odom->getIncrementalTransformation(tcurr, Rcurr, logReader.timestamp, (unsigned char *)logReader.deCompImage->imageData, (unsigned short *)&decompressionBuffer[0]); } }*/ //else // { odom = new DVOdometry; if(logReader.hasMore()) { logReader.getNext(); DVOdometry * dvo = static_cast<DVOdometry *>(odom); dvo->firstRun((unsigned char *)logReader.deCompImage->imageData, (unsigned short *)&decompressionBuffer[0]); } //} ofstream fout1("camera_pose_DVOMarch28.txt"); ofstream fout2("camera_pose_KeyframeMotionMetric0.1March28.txt"); ofstream fout3("loop_closure_transformationMarch28.txt"); ofstream fout4("camera_pose_after_optimizationMarch28.txt"); ofstream fout5("camera_pose_after_optimizationMarch28DVOCov.txt"); ofstream fout6("camera_pose_after_optimizationMarch28DVOLoopTransCov.txt"); /* pcl::visualization::PCLVisualizer cloudViewer; cloudViewer.setBackgroundColor(1, 1, 1); cloudViewer.initCameraParameters(); cloudViewer.addCoordinateSystem(0.1, 0, 0, 0); */ //pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color(cloud->makeShared()); //cloudViewer.addPointCloud<pcl::PointXYZRGB>(cloud->makeShared(), color, "Cloud Viewer"); int loopClosureCount=0; while(logReader.hasMore()) { logReader.getNext(); cv::Mat3b rgbImg(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData); cv::Mat1w depth(height, width, (unsigned short *)&decompressionBuffer[0]); cv::normalize(depth, tmp, 0, 255, cv::NORM_MINMAX, 0); cv::cvtColor(tmp, depthImg, CV_GRAY2RGB); cv::imshow("RGB", rgbImg); cv::imshow("Depth", depthImg); char key = cv::waitKey(1); if(key == 'q') { break; } else if(key == ' ') { key = cv::waitKey(0); } if(key == 'q') { break; } Eigen::Matrix3f Rcurr = Eigen::Matrix3f::Identity(); Eigen::Vector3f tcurr = Eigen::Vector3f::Zero(); // #1 odom->getIncrementalTransformation(tcurr, Rcurr, logReader.timestamp, (unsigned char *)logReader.deCompImage->imageData, (unsigned short *)&decompressionBuffer[0]); fout1<<tcurr[0]<<" "<<tcurr[1]<<" "<<tcurr[2]<<" "<<Rcurr(0,0)<<" "<<Rcurr(0,1)<<" "<<Rcurr(0,2)<<" "<<Rcurr(1,0)<<" "<<Rcurr(1,1)<<" "<<Rcurr(1,2)<<" "<<Rcurr(2,0)<<" "<<Rcurr(2,1)<<" "<<Rcurr(2,2)<<endl; Eigen::Matrix3f Rdelta = Rcurr.inverse() * lastPlaceRecognitionRot; Eigen::Vector3f tdelta = tcurr - lastPlaceRecognitionTrans; //Eigen::MatrixXd covariance = odom->getCovariance(); //Eigen::MatrixXd covariance=Eigen::Matrix<double, 6, 6>::Identity()* 1e-3; if((Projection::rodrigues2(Rdelta).norm() + tdelta.norm()) >= 0.1) { Eigen::MatrixXd covariance = odom->getCovariance(); iSAM.addCameraCameraConstraint(lastTime, logReader.timestamp, lastPlaceRecognitionRot, lastPlaceRecognitionTrans, Rcurr, tcurr); //covariance); printCovariance(fout5, covariance); lastTime = logReader.timestamp; lastPlaceRecognitionRot = Rcurr; lastPlaceRecognitionTrans = tcurr; cout<<"before add keyframe"<<endl; // #2 map.addKeyframe((unsigned char *)logReader.deCompImage->imageData, (unsigned short *)&decompressionBuffer[0], Rcurr, tcurr, logReader.timestamp); fout2<<tcurr[0]<<" "<<tcurr[1]<<" "<<tcurr[2]<<" "<<Rcurr(0,0)<<" "<<Rcurr(0,1)<<" "<<Rcurr(0,2)<<" "<<Rcurr(1,0)<<" "<<Rcurr(1,1)<<" "<<Rcurr(1,2)<<" "<<Rcurr(2,0)<<" "<<Rcurr(2,1)<<" "<<Rcurr(2,2)<<endl; /* //Save keyframe { cv::Mat3b rgbImgKeyframe(height, width, (cv::Vec<unsigned char, 3> *)logReader.deCompImage->imageData); cv::Mat1w depthImgKeyframe(height, width, (unsigned short *)&decompressionBuffer[0]); //save keyframe depth char fileName[1024] = {NULL}; sprintf(fileName, "keyframe_depth_%06d.png", frame_index); cv::imwrite(fileName, depthImgKeyframe); //save keyframe rgb sprintf(fileName, "keyframe_rgb_%06d.png", frame_index); cv::imwrite(fileName, rgbImgKeyframe); frame_index ++; } */ int64_t matchTime; Eigen::Matrix4d transformation; // Eigen::MatrixXd cov(6,6); //isam::Covariance(0.001 * Eigen::Matrix<double, 6, 6>::Identity())) Eigen::MatrixXd cov=0.001 * Eigen::Matrix<double, 6, 6>::Identity(); cout<<"map.addKeyframe is OK"<<endl; // #3 if(placeRecognition.detectLoop((unsigned char *)logReader.deCompImage->imageData, (unsigned short *)&decompressionBuffer[0], logReader.timestamp, matchTime, transformation, cov, loopClosureCount)) { //printCovariance(fout6, cov); cout<<"logReader.timestamp "<<logReader.timestamp<<endl; cout<<"matchTime "<<matchTime<<endl; /* transformation << -0.2913457145219732, 0.228056050293173, -0.9290361201559172, 2.799184934345601, 0.6790194052589797, 0.7333821627861707, -0.03291277242681545, 1.310438143604587, 0.673832562222562, -0.6404225489719699, -0.3685222338703895, 6.988973505496276, 0, 0, 0, 0.999999999999998; */ /* transformation << 0.9998996846969838, 0.003948215234314986, -0.01360265192291004, 0.05847011404293689, -0.004032877285312574, 0.9999726343121815, -0.006202138950136233, 0.04528938486109094, 0.01357779229749574, 0.006256374606648019, 0.9998882444218992, 0.02203456132723125, 0, 0, 0, 1; */ iSAM.addLoopConstraint(logReader.timestamp, matchTime, transformation);//, cov); fout3<<transformation(0,0)<<" "<<transformation(0,1)<<" "<<transformation(0,2)<<" "<<transformation(0,3)<<" "<<transformation(1,0)<<" "<<transformation(1,1)<<" "<<transformation(1,2)<<" "<<transformation(1,3)<<" "<<transformation(2,0)<<" "<<transformation(2,1)<<" "<<transformation(2,2)<<" "<<transformation(2,3)<<" "<<transformation(3,0)<<" "<<transformation(3,1)<<" "<<transformation(3,2)<<" "<<transformation(3,3)<<endl; loopClosureCount++; } } if(loopClosureCount>=1) { break; } } /* for(int i=0; i<loopClosureCount;i++) { iSAM.addLoopConstraint(placeRecognition.loopClosureConstraints.at(i)->time1, placeRecognition.loopClosureConstraints.at(i)->time2, placeRecognition.loopClosureConstraints.at(i)->constraint); }*/ std::vector<std::pair<uint64_t, Eigen::Matrix4f> > posesBefore; iSAM.getCameraPoses(posesBefore); cout<<"It works good before optimization"<<endl; // #4 double residual =iSAM.optimise(); cout<<"It works good after optimize and before map.applyPoses"<<endl; // map.applyPoses(isam); //cout<<"It works good before *cloud=map.getMap and after map.applyPoses(isam)"<<endl; /* pcl::PointCloud<pcl::PointXYZRGB> *cloud = map.getMap(); // Write it back to disk under a different name. // Another possibility would be "savePCDFileBinary()". cout<<"before storing the point cloud map"<<endl; pcl::io::savePCDFileASCII ("outputCloudMap03DVODensity005.pcd", *cloud); cout << "Saved data points to outputMap.pcd." << std::endl; cout<<"copy data into octomap..."<<endl; octomap::ColorOcTree tree( 0.05 ); for (size_t i=0; i<(*cloud).points.size(); i++) { // 将点云里的点插入到octomap中 tree.updateNode( octomap::point3d((*cloud).points[i].x, (*cloud).points[i].y, (*cloud).points[i].z), true ); } for (size_t i=0; i<(*cloud).points.size(); i++) { tree.integrateNodeColor( (*cloud).points[i].x, (*cloud).points[i].y, (*cloud).points[i].z, (*cloud).points[i].r, (*cloud).points[i].g, (*cloud).points[i].b); } tree.updateInnerOccupancy(); tree.write("OctomapColorLab03DVODensity005.ot"); cout<<"please see the done."<<endl; */ //pcl::visualization::PCLVisualizer cloudViewer; // cloudViewer.setBackgroundColor(1, 1, 1); //cloudViewer.initCameraParameters(); // cloudViewer.addCoordinateSystem(0.1, 0, 0, 0); //pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> color(cloud->makeShared()); //cloudViewer.addPointCloud<pcl::PointXYZRGB>(cloud->makeShared(), color, "Cloud Viewer"); std::vector<std::pair<uint64_t, Eigen::Matrix4f> > newPoseGraph; iSAM.getCameraPoses(newPoseGraph); /* for(unsigned int i = 0; i < newPoseGraph.size(); i++) { // file << std::setprecision(6) << std::fixed << (double)newPoseGraph.at(i).first / 1000000.0 << " "; Eigen::Vector3f trans = newPoseGraph.at(i).second.topRightCorner(3, 1); Eigen::Matrix3f rot = newPoseGraph.at(i).second.topLeftCorner(3, 3); fout4 << trans(0) << " " << trans(1) << " " << trans(2) << " "; Eigen::Quaternionf currentCameraRotation(rot); //file << currentCameraRotation.x() << " " << currentCameraRotation.y() << " " << currentCameraRotation.z() << " " << currentCameraRotation.w() << "\n"; }*/ for(std::vector<std::pair<uint64_t, Eigen::Matrix4f> >::iterator ite=newPoseGraph.begin(); ite!=newPoseGraph.end(); ite++) { Eigen::Matrix3f Roptimized; Roptimized<<ite->second(0,0), ite->second(0,1), ite->second(0,2), ite->second(1,0), ite->second(1,1), ite->second(1,2), ite->second(2,0), ite->second(2,1), ite->second(2,2); Eigen::Quaternionf quatOptimized(Roptimized); fout4<<ite->second(0,3)<<" "<<ite->second(1,3)<<" "<<ite->second(2,3)<<" "<<quatOptimized.w()<<" "<<quatOptimized.x()<<" "<<quatOptimized.y()<<" "<<quatOptimized.z()<<endl; } cout<<"The number of optimized poses"<<newPoseGraph.size()<<endl; // drawPoses(poses, cloudViewer, 1.0, 0, 0); //drawPoses(posesBefore, cloudViewer, 0, 0, 1.0); //cloudViewer.spin(); delete [] decompressionBuffer; return 0; }