bool operator()(I1 begin1, S1 end1, I2 begin2, S2 end2,
C pred_ = C{}, P1 proj1_ = P1{}, P2 proj2_ = P2{}) const
{
auto &&pred = as_function(pred_);
auto &&proj1 = as_function(proj1_);
auto &&proj2 = as_function(proj2_);
for(; begin2 != end2; ++begin1)
{
if(begin1 == end1 || pred(proj2(*begin2), proj1(*begin1)))
return false;
if(!pred(proj1(*begin1), proj2(*begin2)))
++begin2;
}
return true;
}
bool collides(Entity* const entityOne, Entity* const entityTwo)
{//Function which uses the separating axis theorem to detect for collisions between two entities
//Projection getProjection(const sf::Vector2f&, const Square&);
std::vector<sf::Vector2f> axes1(entityOne->getVertexCount());
std::vector<sf::Vector2f> axes2(entityTwo->getVertexCount());
for (int i(0); i < entityOne->getVertexCount(); ++i)
{//Loop through the first shape and get all normals to each side
int index(0);
(i + 1) == entityOne->getVertexCount() ? index = 0 : index = i + 1;
axes1[i] = entityOne->getNormal(i, index);
}
for (int i(0); i < entityTwo->getVertexCount(); ++i)
{//Loop through the second shape and get all normals to each side
int index(0);
(i + 1) == entityTwo->getVertexCount() ? index = 0 : index = i + 1;
axes2[i] = entityTwo->getNormal(i, index);
}
for (int i(0); i < axes1.size(); ++i)
{//Project shape2 onto shape1's axis and determine if there's a gap
sf::Vector2f normal(axes1[i]);
SATProjection proj1(getProjection(normal, entityOne)); //max and minimum values of the first shape projection
SATProjection proj2(getProjection(normal, entityTwo)); //max and minimum values of the second shape projection
if (!overlaps(proj1, proj2))
return false;
}
for (int i(0); i < axes2.size(); ++i)
{
sf::Vector2f normal(axes2[i]);
SATProjection proj1(getProjection(normal, entityOne)); //max and minimum values of the first shape projection
SATProjection proj2(getProjection(normal, entityTwo)); //max and minimum values of the second shape projection
if (!overlaps(proj1, proj2))
return false;
}
return(true);
}
bool lexicographical_compare(I1 first1, S1 last1, I2 first2, S2 last2,
Comp comp_ = Comp{}, Proj1 proj1_ = Proj1{},
Proj2 proj2_ = Proj2{}) {
auto&& comp = __stl2::as_function(comp_);
auto&& proj1 = __stl2::as_function(proj1_);
auto&& proj2 = __stl2::as_function(proj2_);
for (; first1 != last1 && first2 != last2; ++first1, ++first2) {
if (comp(proj1(*first1), proj2(*first2))) {
return true;
}
if (comp(proj2(*first2), proj1(*first1))) {
return false;
}
}
return first1 == last1 && first2 != last2;
}
static bool four_iter_impl(I1 begin1, S1 end1, I2 begin2, S2 end2, C pred_, P1 proj1_,
P2 proj2_)
{
auto &&pred = as_function(pred_);
auto &&proj1 = as_function(proj1_);
auto &&proj2 = as_function(proj2_);
// shorten sequences as much as possible by lopping of any equal parts
for(; begin1 != end1 && begin2 != end2; ++begin1, ++begin2)
if(!pred(proj1(*begin1), proj2(*begin2)))
goto not_done;
return begin1 == end1 && begin2 == end2;
not_done:
// begin1 != end1 && begin2 != end2 && *begin1 != *begin2
auto l1 = distance(begin1, end1);
auto l2 = distance(begin2, end2);
if(l1 != l2)
return false;
// For each element in [f1, l1) see if there are the same number of
// equal elements in [f2, l2)
for(I1 i = begin1; i != end1; ++i)
{
// Have we already counted the number of *i in [f1, l1)?
for(I1 j = begin1; j != i; ++j)
if(pred(proj1(*j), proj1(*i)))
goto next_iter;
{
// Count number of *i in [f2, l2)
iterator_difference_t<I1> c2 = 0;
for(I2 j = begin2; j != end2; ++j)
if(pred(proj1(*i), proj2(*j)))
++c2;
if(c2 == 0)
return false;
// Count number of *i in [i, l1) (we can start with 1)
iterator_difference_t<I1> c1 = 1;
for(I1 j = next(i); j != end1; ++j)
if(pred(proj1(*i), proj1(*j)))
++c1;
if(c1 != c2)
return false;
}
next_iter:;
}
return true;
}
void reconstruct(Mat& K, Mat& R, Mat& T, vector<Point2f>& p1, vector<Point2f>& p2, Mat& structure)
{
Mat proj1(3, 4, CV_32FC1);
Mat proj2(3, 4, CV_32FC1);
proj1(Range(0, 3), Range(0, 3)) = Mat::eye(3, 3, CV_32FC1);
proj1.col(3) = Mat::zeros(3, 1, CV_32FC1);
R.convertTo(proj2(Range(0, 3), Range(0, 3)), CV_32FC1);
T.convertTo(proj2.col(3), CV_32FC1);
Mat fK;
K.convertTo(fK, CV_32FC1);
proj1 = fK*proj1;
proj2 = fK*proj2;
triangulatePoints(proj1, proj2, p1, p2, structure);
}
T operator()(I1 begin1, S1 end1, I2 begin2, S2 end2, T init, BOp1 bop1_ = BOp1{},
BOp2 bop2_ = BOp2{}, P1 proj1_ = P1{}, P2 proj2_ = P2{}) const
{
auto &&bop1 = as_function(bop1_);
auto &&bop2 = as_function(bop2_);
auto &&proj1 = as_function(proj1_);
auto &&proj2 = as_function(proj2_);
for (; begin1 != end1 && begin2 != end2; ++begin1, ++begin2)
init = bop1(init, bop2(proj1(*begin1), proj2(*begin2)));
return init;
}
tagged_pair<tag::in1(I1), tag::in2(I2)>
operator()(I1 begin1, S1 end1, I2 begin2, S2 end2, C pred_ = C{}, P1 proj1_ = P1{},
P2 proj2_ = P2{}) const
{
auto &&pred = as_function(pred_);
auto &&proj1 = as_function(proj1_);
auto &&proj2 = as_function(proj2_);
for(; begin1 != end1 && begin2 != end2; ++begin1, ++begin2)
if(!pred(proj1(*begin1), proj2(*begin2)))
break;
return {begin1, begin2};
}
T operator()(I1 begin1, S1 end1, I2 begin2, T init, BOp1 bop1_ = BOp1{},
BOp2 bop2_ = BOp2{}, P1 proj1_ = P1{}, P2 proj2_ = P2{}) const
{
auto &&bop1 = invokable(bop1_);
auto &&bop2 = invokable(bop2_);
auto &&proj1 = invokable(proj1_);
auto &&proj2 = invokable(proj2_);
for (; begin1 != end1; ++begin1, ++begin2)
init = bop1(init, bop2(proj1(*begin1), proj2(*begin2)));
return init;
}
std::pair<I1, I2>
operator()(I1 begin1, S1 end1, I2 begin2, C pred_ = C{}, P1 proj1_ = P1{},
P2 proj2_ = P2{}) const
{
auto &&pred = as_function(pred_);
auto &&proj1 = as_function(proj1_);
auto &&proj2 = as_function(proj2_);
for(; begin1 != end1; ++begin1, ++begin2)
if(!pred(proj1(*begin1), proj2(*begin2)))
break;
return {begin1, begin2};
}
bool includes(I1 first1, S1 last1, I2 first2, S2 last2, Comp comp_ = Comp{},
Proj1 proj1_ = Proj1{}, Proj2 proj2_ = Proj2{}) {
auto&& comp = __stl2::as_function(comp_);
auto&& proj1 = __stl2::as_function(proj1_);
auto&& proj2 = __stl2::as_function(proj2_);
while (true) {
if (first2 == last2) {
return true;
}
if (first1 == last1) {
return false;
}
if (comp(proj2(*first2), proj1(*first1))) {
return false;
}
if (!comp(proj1(*first1), proj2(*first2))) {
++first2;
}
++first1;
}
}
mismatch(I1 first1, S1 last1, I2 first2, Pred pred_ = Pred{},
Proj1 proj1_ = Proj1{}, Proj2 proj2_ = Proj2{}) {
auto&& pred = __stl2::as_function(pred_);
auto&& proj1 = __stl2::as_function(proj1_);
auto&& proj2 = __stl2::as_function(proj2_);
for (; first1 != last1; ++first1, ++first2) {
if (!pred(proj1(*first1), proj2(*first2))) {
break;
}
}
return {first1, first2};
}
PxGeometry& Camera::getPixelFrustum(FDreal pixelXSize, FDreal pixelYSize) {
if (pixelFrustum.isValid()) {
return pixelFrustum;
}
Vector3 proj1(-pixelXSize / 2, -pixelYSize / 2, 1);
Vector3 proj2(-pixelXSize / 2, pixelYSize / 2, 1);
Vector3 proj3(pixelXSize / 2, -pixelYSize / 2, 1);
Vector3 proj4(pixelXSize / 2, pixelYSize / 2, 1);
fdmath::Matrix44 projInverse;
fdmath::gluInvertMatrix44(projection, projInverse);
FDreal len = -100.0f;
Vector3 view1 = projInverse.transform(proj1).getNormalized() * len;
Vector3 view2 = projInverse.transform(proj2).getNormalized() * len;
Vector3 view3 = projInverse.transform(proj3).getNormalized() * len;
Vector3 view4 = projInverse.transform(proj4).getNormalized() * len;
static const PxVec3 convexVerts[] = {PxVec3(0,0,0), view1,
view2, view3, view4};
PhysicsSystem* physics = FreeThread__getWorld().
getSystem<PhysicsSystem>();
PxConvexMeshDesc convexDesc;
convexDesc.points.count = 5;
convexDesc.points.stride = sizeof(PxVec3);
convexDesc.points.data = convexVerts;
convexDesc.flags = PxConvexFlag::eCOMPUTE_CONVEX;
convexDesc.vertexLimit = 256;
PxDefaultMemoryOutputStream buf;
if (!physics->cooking->cookConvexMesh(convexDesc, buf)) {
FD_THROW(GenericException("Unable to cook convex pixel mesh!"));
}
PxDefaultMemoryInputData input(buf.getData(), buf.getSize());
PxConvexMesh* convexMesh = physics->physics->createConvexMesh(input);
pixelFrustum = PxConvexMeshGeometry(convexMesh);
return pixelFrustum;
}
cv::Mat TestProjection::test(double userX, double userY, double userZ,
const char* filename) {
//Coordinates of the projection in the real world
/*cv::Point3f p11(-480, 735, -420);
cv::Point3f p12(0, 935, 0);
cv::Point3f p13(0, 220, 0);
cv::Point3f p14(-480, 240, -420);
Plane3d proj1(p11, p12, p13, p14);
cv::Point3f p21(0, 935, 0);
cv::Point3f p22(480, 735, -420);
cv::Point3f p23(480, 240, -420);
cv::Point3f p24(0, 220, 0);
Plane3d proj2(p21, p22, p23, p24);*/
cv::Point3f p11(-590, 725, -350);
cv::Point3f p12(0, 955, 0);
cv::Point3f p13(0, 200, 0);
cv::Point3f p14(-590, 227, -350);
Plane3d proj1(p11, p12, p13, p14);
cv::Point3f p21(0, 955, 0);
cv::Point3f p22(567, 755, -350);
cv::Point3f p23(567, 227, -350);
cv::Point3f p24(0, 200, 0);
Plane3d proj2(p21, p22, p23, p24);
std::vector<Plane3d> planes;
planes.push_back(proj1);
planes.push_back(proj2);
Projection proj(planes);
// proj.print();
//Create the user with the obtained projection coordinates
User u(proj);
//Update his position
u.updatePosition(userX, userY, userZ);
// u.print();
//Create the distorted-corrected plane pairs, using the projections
//on the user's view plane
//Plane 1
//****************************************************************************************************
Plane2d p1 = u.getProjectedPlanes().at(0).to2d();
Plane2d p2(cv::Point2f(0, 0), cv::Point2f(480, 0), cv::Point2f(480, 540), cv::Point2f(0, 540));
// Plane2d p2(cv::Point2f(0, 0), cv::Point2f(230, 0), cv::Point2f(230, 520), cv::Point2f(0, 520));
// Plane2d p2(cv::Point2f(0, 0), cv::Point2f(270, 0), cv::Point2f(270, 405), cv::Point2f(0, 405));
//****************************************************************************************************
//Invert the plane y coordinates
Plane2d inv1 = p1.yInverted();
//Move it so that it's closer to the target plane
cv::Vec2f dist = pjs::distance(inv1, p2);
Plane2d pp1(cv::Point2f(inv1.getPoint(0).x - dist[0], inv1.getPoint(0).y - dist[1]),
cv::Point2f(inv1.getPoint(1).x - dist[0], inv1.getPoint(1).y - dist[1]),
cv::Point2f(inv1.getPoint(2).x - dist[0], inv1.getPoint(2).y - dist[1]),
cv::Point2f(inv1.getPoint(3).x - dist[0], inv1.getPoint(3).y - dist[1]));
//Plane 2
//****************************************************************************************************
Plane2d p3 = u.getProjectedPlanes().at(1).to2d();
Plane2d p4(cv::Point2f(0, 0), cv::Point2f(480, 0), cv::Point2f(480, 540), cv::Point2f(0, 540));
// Plane2d p4(cv::Point2f(0, 0), cv::Point2f(230, 0), cv::Point2f(230, 520), cv::Point2f(0, 520));
// Plane2d p4(cv::Point2f(0, 0), cv::Point2f(270, 0), cv::Point2f(270, 405), cv::Point2f(0, 405));
//****************************************************************************************************
//Invert the plane y coordinates
Plane2d inv2 = p3.yInverted();
//Move it so that it's closer to the target plane
dist = pjs::distance(inv2, p4);
Plane2d pp3(cv::Point2f(inv2.getPoint(0).x - dist[0], inv2.getPoint(0).y - dist[1]),
cv::Point2f(inv2.getPoint(1).x - dist[0], inv2.getPoint(1).y - dist[1]),
cv::Point2f(inv2.getPoint(2).x - dist[0], inv2.getPoint(2).y - dist[1]),
cv::Point2f(inv2.getPoint(3).x - dist[0], inv2.getPoint(3).y - dist[1]));
//***********************
//Load the target image
//***********************
cv::Mat img = cv::imread(filename, CV_LOAD_IMAGE_COLOR);
if (!img.data) {
std::cout << " --(!) Error reading image" << std::endl;
throw std::exception();
}
//Helper object
Utils utils;
//Divide the image in two
// std::vector<cv::Mat> images = utils.divideImageInTwo(img);
//Build the surfaces with their reference planes and their corresponding
//image
Surface s1(pp1, p2);
Surface s2(pp3, p4);
std::vector<Surface*> surfaces;
surfaces.push_back(&s1);
surfaces.push_back(&s2);
int originX;
int padding;
int screenWidth = 1280;
int screenHeight = 800;
//TODO recursive position correction
int width1 = s1.getWidth();
int width2 = s2.getWidth();
int diffW = width1 - width2;
if (diffW < 0) {
originX = screenWidth / 2 - width1;
padding = 0;
} else {
originX = 0 + screenWidth / 2 - width1;
padding = 0;
}
//1st position correction
cv::Point2f origin(originX, 0);
s1.correctBBPosition(origin);
cv::Point2f s1ur = s1.getUpperRightCorner();
s2.correctPosition(s1ur);
cv::Point2f upperLeft = s2.getUpperLeftCorner();
cv::Point2f upperRight = s2.getUpperRightCorner();
double topY;
if (upperLeft.y < upperRight.y) {
topY = upperLeft.y;
} else {
topY = upperRight.y;
}
cv::Size size = utils.getFinalSize(surfaces);
int diffH = screenHeight - size.height;
//2nd position correction if necessary (if second plane is still outside)
if (!topY < 0) {
topY = 0;
}
cv::Point2f newOrigin(originX, -topY + diffH / 2);
s1.correctBBPosition(newOrigin);
s1ur = s1.getUpperRightCorner();
s2.correctPosition(s1ur);
// cv::Size size = utils.getFinalSize(surfaces);
size.width += padding;
size.width = std::max(screenWidth, size.width);
size.height = screenHeight;
cv::Size sizeS1(size.width / 2, size.height);
s1.setSize(sizeS1);
s2.setSize(size);
std::vector<cv::Mat> images = utils.divideImageInTwo(img);
s1.setImage(images.at(0));
s2.setImage(images.at(1));
s1.applyHomography();
s2.applyHomography();
// s1.addTransparency();
// s2.addTransparency();
cv::Mat finalImage = utils.getImageFromSurfaces(surfaces);
surfaces.clear();
return finalImage;
}
int main ()
{
SingleParticle2dx::ConfigContainer* config = SingleParticle2dx::ConfigContainer::Instance();
int n = config->getParticleSize();
config->setProjectionMethod(4);
config->setCacheProjections(false);
config->setParallelProjection(false);
config->setProjectionMaskingMethod(0);
config->setRefinementMethod(0);
config->setTrialAngleGenerator(4);
config->setBackprojectionMethod(0);
config->setLPProjectionRadius(n);
SingleParticle2dx::DataStructures::Reconstruction3d rec3d(n,n,n);
SingleParticle2dx::Utilities::UtilityFunctions::generateInitialModelForInitRef(rec3d);
rec3d.scale(1/1.34);
SingleParticle2dx::Utilities::UtilityFunctions::setProgressBar( 33 );
SingleParticle2dx::DataStructures::ParticleContainer c_dummy;
rec3d.forceProjectionPreparation(c_dummy);
SingleParticle2dx::DataStructures::Projection2d proj1(n,n);
SingleParticle2dx::DataStructures::Projection2d proj2(n,n);
SingleParticle2dx::DataStructures::Projection2d proj3(n,n);
SingleParticle2dx::DataStructures::Orientation o1(0,0,0);
SingleParticle2dx::DataStructures::Orientation o2(0,90,0);
SingleParticle2dx::DataStructures::Orientation o3(0,90,90);
rec3d.calculateProjection(o1, proj1);
rec3d.calculateProjection(o2, proj2);
rec3d.calculateProjection(o3, proj3);
if(config->getShowSights())
{
proj1.setMidddleTarget();
proj2.setMidddleTarget();
proj3.setMidddleTarget();
}
SingleParticle2dx::Utilities::UtilityFunctions::setProgressBar( 66 );
std::string cont_folder_name = config->getContainerName() + "/Div_output/";
std::string filename_p1 = cont_folder_name + "init_topview.mrc";
std::string filename_p2 = cont_folder_name + "init_sideview1.mrc";
std::string filename_p3 = cont_folder_name + "init_sideview2.mrc";
proj1.writeToFile(filename_p1);
proj2.writeToFile(filename_p2);
proj3.writeToFile(filename_p3);
SingleParticle2dx::Utilities::UtilityFunctions::generateImageOutput(filename_p1, "Top View", config->getScriptName(), false, false);
SingleParticle2dx::Utilities::UtilityFunctions::generateImageOutput(filename_p2, "Side View X", config->getScriptName(), false, false);
SingleParticle2dx::Utilities::UtilityFunctions::generateImageOutput(filename_p3, "Side View Y", config->getScriptName(), false, false);
rec3d.writeToFile(config->getContainerName() + "/Rec_3d/Init3DFromMRC.map");
SingleParticle2dx::Utilities::UtilityFunctions::generateImageOutput(config->getContainerName() + "/Rec_3d/Init3DFromMRC.map", "Initial 3D Reference", config->getScriptName(), true, true);
SingleParticle2dx::Utilities::UtilityFunctions::setProgressBar( 100 );
return 0;
}
/**
* @brief Calculates the view point of the newFrame relative to the oldFrames using pcl::estimateRigidTransformationSVD and RANSAC.
* @return True if a good position has been found.
*/
bool utils::pclRigidTransform(const std::vector<KeyframeContainer>& oldFrames,KeyframeContainer& newFrame)
{
//fill point clouds with all matches and another set with the best matches
pcl::PointCloud<pcl::PointXYZ> cloud1,cloud2;
std::vector<cv::Mat_<double> > points2D,points3D;
pcl::PointCloud<pcl::PointXYZ> completeCloud1,completeCloud2;
std::vector<cv::Mat_<double> > completePoints2D,completePoints3D;
//precalculate the extended projection matrices
cv::Mat_<double> proj2(3,4,0.0);
std::vector<cv::Mat_<double> > proj1Collected;
for(uint i=0;i<oldFrames.size();++i)
{
cv::Mat_<double> proj1E;
if(oldFrames[i].projectionMatrix.rows>0 && oldFrames[i].projectionMatrix.cols>0)
{
proj1E=cv::Mat_<double>(4,4,0.0);
for(int x=0;x<3;x++) for(int y=0;y<4;y++)
proj1E(x,y)=oldFrames[i].projectionMatrix(x,y);
proj1E(3,3)=1;
}
proj1Collected.push_back(proj1E);
}
//save all the matched points and the best matches
for(uint me=0;me<newFrame.matches.size();++me)
{
//search the best match to the current best match by comparing match quality
uint currentBestMatchIndex=0;
double currentBestQuality=INFINITY;
uint tmpIndex=0;
for(uint you=0;you<newFrame.matches[me].size();++you)
{
if(currentBestQuality>newFrame.matches[me][you].second)
{
//find the Frame that the keypoint belongs to
tmpIndex=0;
while(oldFrames[tmpIndex].ID!=newFrame.matches[me][you].first.val[0])
++tmpIndex;
//if this frame hasn't been flagged invalid use this keypoint
if(!oldFrames[tmpIndex].invalid)
{
currentBestMatchIndex=you;
currentBestQuality=newFrame.matches[me][you].second;
}
}
}
//calculate all the points from the matches
for(uint you=0;you<newFrame.matches[me].size();++you)
{
//we only know the ID of the frame and need to find the index
uint currentFrame1Index=0;
while(oldFrames[currentFrame1Index].ID!=newFrame.matches[me][you].first.val[0])
++currentFrame1Index;
//add points only if the frame is valid
if(!oldFrames[currentFrame1Index].invalid)
{
//get the 2D points from both frames
cv::Point2f p1=oldFrames[currentFrame1Index].keypoints[newFrame.matches[me][you].first.val[1]].pt;
cv::Point2f p2=newFrame.keypoints[me].pt;
//get the depth information from both frames
float depth1=oldFrames[currentFrame1Index].depthImg.image.at<float>(p1.y,p1.x);
float depth2=newFrame.depthImg.image.at<float>(p2.y,p2.x);
//if both depths are available and the projectivity matrix is valid
if(isnan(depth1)==0 && isnan(depth2)==0 && proj1Collected[currentFrame1Index].rows>0 && proj1Collected[currentFrame1Index].cols>0)
{
//calculate the 2D and 3D point of the first frame
pcl::PointXYZ tmpPoint1;
cv::Mat_<double> tmpPoint2D(2,1,0.0);
tmpPoint2D(0,0)=p1.x;
tmpPoint2D(1,0)=p1.y;
cv::Mat_<double> tmpPointHomo3D=reprojectImagePointTo3D(tmpPoint2D,oldFrames[currentFrame1Index].cameraCalibration,proj1Collected[currentFrame1Index],depth1);
//convert 3D point to pcl format
tmpPoint1.x=tmpPointHomo3D(0,0);
tmpPoint1.y=tmpPointHomo3D(1,0);
tmpPoint1.z=tmpPointHomo3D(2,0);
//calculate the 3D point of the second frame
pcl::PointXYZ tmpPoint2;
cv::Mat_<double> tmpP2D(3,1,1.0);
tmpP2D(0,0)=p2.x;
tmpP2D(1,0)=p2.y;
tmpP2D=utils::reprojectImagePointTo2DNormalised(tmpP2D,newFrame.cameraCalibration);
//convert 3D point to pcl format
tmpPoint2.x=tmpP2D(0,0)*depth2;
tmpPoint2.y=tmpP2D(1,0)*depth2;
tmpPoint2.z=depth2;
//put the match in the complete point cloud
completeCloud1.push_back(tmpPoint1);
completePoints3D.push_back(tmpPointHomo3D);
completeCloud2.push_back(tmpPoint2);
completePoints2D.push_back(tmpP2D);
//if you is the best match put it into the corresponding clouds
if(you==currentBestMatchIndex)
{
cloud1.push_back(tmpPoint1);
points3D.push_back(tmpPointHomo3D);
cloud2.push_back(tmpPoint2);
points2D.push_back(tmpP2D);
}
}
}
}
}
#ifndef NDEBUG
ROS_INFO("cloud size for ransac: %lu",cloud1.points.size());
#endif
//RANSAC only if there are some points available
if(cloud1.size()>20)
{
Eigen::Matrix4f transformation;
const double REPROJECTION_THRESHOLD_SQUARE=1e-4;
const double DESIRED_CONFIDENCE_LOG=std::log(0.01);
const int MAX_ITERATIONS=200;
double iterationsNeeded=MAX_ITERATIONS;
int iteration=0;
std::vector<int> inliers;
std::vector<int> bestInliers;
std::vector<int> randomIndices;
cv::RNG Rander(std::time(NULL));
cv::Mat_<double> tP;
cv::Mat_<double> proj2(3,4,0.0);
//do adaptive RANSAC on the best match clouds
while((double)iteration<std::ceil(iterationsNeeded))
{
//get random indices
randomIndices.clear();
while(randomIndices.size()<3)
{
int newIndex=Rander.uniform(0,(int) cloud1.points.size());
for(uint ind=0;ind<randomIndices.size();ind++)
if(randomIndices[ind]==newIndex) newIndex=-1;
if(newIndex>=0) randomIndices.push_back(newIndex);
}
//estimate the transform
pcl::estimateRigidTransformationSVD(cloud1,randomIndices,cloud2,randomIndices,transformation);
//convert projection matrix to opencv format
for(int x=0;x<3;x++) for(int y=0;y<4;y++)
proj2(x,y)=transformation(x,y);
//estimate the inliers
inliers.clear();
for(int i=0;i<(int)points3D.size();i++)
{
tP=proj2*points3D[i];
tP/=tP(2,0);
tP-=points2D[i];
if(tP(0,0)*tP(0,0)+tP(1,0)*tP(1,0)<REPROJECTION_THRESHOLD_SQUARE)
inliers.push_back(i);
}
//save the solution if it's better than before
if(inliers.size()>bestInliers.size())
{
bestInliers=inliers;
iterationsNeeded=DESIRED_CONFIDENCE_LOG/std::log(1-std::pow(((double)bestInliers.size())/((double)cloud1.points.size()),3));
}
iteration++;
}
#ifndef NDEBUG
ROS_INFO("inlier ratio: %f",((double)bestInliers.size())/((double)cloud1.points.size()));
#endif
//calculate final solution from the complete point clouds only if enough inliers have been found and the inlier ration is high enough
if(bestInliers.size()>9 && ((double)bestInliers.size())/((double)cloud1.points.size())>0.1)
{
//reestimate the best transformation from the reduced point cloud
pcl::estimateRigidTransformationSVD(cloud1,bestInliers,cloud2,bestInliers,transformation);
for(int x=0;x<3;x++) for(int y=0;y<4;y++)
proj2(x,y)=transformation(x,y);
//find all inliers in the complete point cloud
inliers.clear();
for(int i=0;i<(int)completePoints3D.size();i++)
{
tP=proj2*completePoints3D[i];
tP/=tP(2,0);
tP-=completePoints2D[i];
if(tP(0,0)*tP(0,0)+tP(1,0)*tP(1,0)<REPROJECTION_THRESHOLD_SQUARE)
inliers.push_back(i);
}
//estimate the transform from the complete cloud
pcl::estimateRigidTransformationSVD(completeCloud1,inliers,completeCloud2,inliers,transformation);
for(int x=0;x<3;x++) for(int y=0;y<4;y++)
proj2(x,y)=transformation(x,y);
newFrame.projectionMatrix=proj2;
return true;
}
}
return false;
}
