4 #include <boost/numeric/ublas/symmetric.hpp>
5 #include <boost/numeric/ublas/io.hpp>
10 #include <QStringList>
20 #include "ControlNetFile.h"
21 #include "ControlNetFile.h"
22 #include "ControlNetFileV0002.pb.h"
32 using boost::numeric::ublas::symmetric_matrix;
33 using boost::numeric::ublas::upper;
43 ControlPoint::ControlPoint() : invalid(false) {
59 referenceMeasure = NULL;
73 referenceMeasure = NULL;
81 QListIterator<QString> i(*other.cubeSerials);
83 QString sn = i.next();
89 if (other.referenceMeasure == otherCm)
126 referenceMeasure = NULL;
131 id = fileEntry.id().c_str();
137 dateTime = fileEntry.datetime().c_str();
142 switch (fileEntry.type()) {
143 case ControlPointFileEntryV0002_PointType_obsolete_Tie:
144 case ControlPointFileEntryV0002_PointType_Free:
147 case ControlPointFileEntryV0002_PointType_Constrained:
150 case ControlPointFileEntryV0002_PointType_obsolete_Ground:
151 case ControlPointFileEntryV0002_PointType_Fixed:
155 QString msg =
"Point type is invalid.";
159 ignore = fileEntry.ignore();
163 if (fileEntry.has_apriorisurfpointsource()) {
164 switch (fileEntry.apriorisurfpointsource()) {
165 case ControlPointFileEntryV0002_AprioriSource_None:
169 case ControlPointFileEntryV0002_AprioriSource_User:
173 case ControlPointFileEntryV0002_AprioriSource_AverageOfMeasures:
177 case ControlPointFileEntryV0002_AprioriSource_Reference:
181 case ControlPointFileEntryV0002_AprioriSource_Basemap:
185 case ControlPointFileEntryV0002_AprioriSource_BundleSolution:
189 case ControlPointFileEntryV0002_AprioriSource_Ellipsoid:
190 case ControlPointFileEntryV0002_AprioriSource_DEM:
195 if (fileEntry.has_apriorisurfpointsourcefile()) {
199 if (fileEntry.has_aprioriradiussource()) {
200 switch (fileEntry.aprioriradiussource()) {
201 case ControlPointFileEntryV0002_AprioriSource_None:
204 case ControlPointFileEntryV0002_AprioriSource_User:
207 case ControlPointFileEntryV0002_AprioriSource_AverageOfMeasures:
210 case ControlPointFileEntryV0002_AprioriSource_Ellipsoid:
213 case ControlPointFileEntryV0002_AprioriSource_DEM:
216 case ControlPointFileEntryV0002_AprioriSource_BundleSolution:
220 case ControlPointFileEntryV0002_AprioriSource_Reference:
221 case ControlPointFileEntryV0002_AprioriSource_Basemap:
226 if (fileEntry.has_aprioriradiussourcefile()) {
232 if (fileEntry.has_apriorix() && fileEntry.has_aprioriy() &&
233 fileEntry.has_aprioriz()) {
239 if (fileEntry.aprioricovar_size() > 0) {
240 symmetric_matrix<double, upper> covar;
243 covar(0, 0) = fileEntry.aprioricovar(0);
244 covar(0, 1) = fileEntry.aprioricovar(1);
245 covar(0, 2) = fileEntry.aprioricovar(2);
246 covar(1, 1) = fileEntry.aprioricovar(3);
247 covar(1, 2) = fileEntry.aprioricovar(4);
248 covar(2, 2) = fileEntry.aprioricovar(5);
253 if (fileEntry.latitudeconstrained())
255 if (fileEntry.longitudeconstrained())
257 if (fileEntry.radiusconstrained())
261 if (fileEntry.latitudeconstrained())
263 if (fileEntry.radiusconstrained())
271 if (fileEntry.has_adjustedx() &&
272 fileEntry.has_adjustedy() &&
273 fileEntry.has_adjustedz()) {
279 if (fileEntry.adjustedcovar_size() > 0) {
280 symmetric_matrix<double, upper> covar;
283 covar(0, 0) = fileEntry.adjustedcovar(0);
284 covar(0, 1) = fileEntry.adjustedcovar(1);
285 covar(0, 2) = fileEntry.adjustedcovar(2);
286 covar(1, 1) = fileEntry.adjustedcovar(3);
287 covar(1, 2) = fileEntry.adjustedcovar(4);
288 covar(2, 2) = fileEntry.adjustedcovar(5);
302 for (
int m = 0 ; m < fileEntry.measures_size() ; m++) {
307 if (fileEntry.has_referenceindex()) {
308 SetRefMeasure((*measures)[cubeSerials->at(fileEntry.referenceindex())]);
324 referenceMeasure = NULL;
345 if (measures != NULL) {
347 for (
int i = 0; i < keys.size(); i++) {
348 delete(*measures)[keys[i]];
349 (*measures)[keys[i]] = NULL;
361 referenceMeasure = NULL;
434 QString msg =
"The SerialNumber is not unique. A measure with "
436 "exists for ControlPoint [" +
GetId() +
"]";
441 if (!measures->size()) {
442 ASSERT(referenceMeasure == NULL);
443 referenceMeasure = measure;
445 else if (referenceMeasure->IsIgnored() && !measure->IsIgnored() &&
450 referenceMeasure = measure;
455 measures->insert(newSerial, measure);
456 cubeSerials->append(newSerial);
474 if (!measures->contains(serialNumber)) {
475 QString msg =
"No measure with serial number [" + serialNumber +
476 "] is owned by this point";
493 return ControlMeasure::MeasureLocked;
496 measures->remove(serialNumber);
497 cubeSerials->removeAt(cubeSerials->indexOf(serialNumber));
500 if (cubeSerials->size()) {
501 if (referenceMeasure == cm) {
502 referenceMeasure = (*measures)[cubeSerials->at(0)];
507 referenceMeasure = NULL;
514 if (!IsIgnored() && !cm->IsIgnored())
523 return ControlMeasure::Success;
546 if (index < 0 || index >= cubeSerials->size()) {
547 QString msg =
"index [" + QString(index) +
"] out of bounds";
551 return Delete(cubeSerials->at(index));
584 return (*measures)[serialNumber];
596 return measures->value(serialNumber);
601 if (index < 0 || index >= cubeSerials->size()) {
602 QString msg =
"Index [" +
toString(index) +
"] out of range";
611 if (index < 0 || index >= cubeSerials->size()) {
612 QString msg =
"Index [" +
toString(index) +
"] out of range";
626 if (referenceMeasure == NULL) {
627 QString msg =
"Control point [" +
GetId() +
"] has no reference measure!";
631 return referenceMeasure;
639 if (referenceMeasure == NULL) {
640 QString msg =
"Control point [" +
GetId() +
"] has no reference measure!";
644 return referenceMeasure;
750 if (index < 0 || index >= cubeSerials->size()) {
751 QString msg =
"Index [";
752 msg +=
toString(index) +
"] out of range";
770 if (!cubeSerials->contains(sn)) {
771 QString msg =
"Point [" +
id +
"] has no measure with serial number [" +
801 referenceMeasure = measure;
819 if (oldStatus !=
ignore) {
823 if (!cm->IsIgnored()) {
869 QString msg =
"Invalid Point Enumeration, [" + QString(
type) +
"], for "
870 "Control Point [" +
GetId() +
"]";
890 RadiusSource::Source source) {
908 QString sourceFile) {
938 if (aprioriSP.GetLatSigma().
isValid())
940 if (aprioriSP.GetLonSigma().
isValid())
942 if (aprioriSP.GetLocalRadiusSigma().
isValid())
958 SurfacePointSource::Source source) {
975 QString sourceFile) {
1026 QString msg =
"ControlPoint [" +
GetId() +
"] is a fixed point ";
1027 msg +=
"and requires an apriori x/y/z";
1038 int goodMeasures = 0;
1041 for (
int j = 0; j < cubeSerials->size(); j++) {
1050 if (m->IsIgnored()) {
1054 Camera *cam = m->Camera();
1056 QString msg =
"The Camera must be set prior to calculating apriori";
1059 if (cam->
SetImage(m->GetSample(), m->GetLine())) {
1066 r2B += pB[0]*pB[0] + pB[1]*pB[1] + pB[2]*pB[2];
1100 || NumberOfConstrainedCoordinates() == 3
1101 || IsLatitudeConstrained()
1102 || IsLongitudeConstrained()
1103 || IsRadiusConstrained()) {
1112 if (goodMeasures == 0) {
1113 QString msg =
"ControlPoint [" +
GetId() +
"] has no measures which "
1114 "project to lat/lon/radius (x/y/z)";
1120 if (
GetType() ==
Free || NumberOfConstrainedCoordinates() == 0) {
1121 double avgX = xB / goodMeasures;
1122 double avgY = yB / goodMeasures;
1123 double avgZ = zB / goodMeasures;
1124 double avgR2 = r2B / goodMeasures;
1125 double scale = sqrt(avgR2/(avgX*avgX+avgY*avgY+avgZ*avgZ));
1180 for (
int j = 0; j < keys.size(); j++) {
1190 Camera *cam = m->Camera();
1208 if (!fpmap->
SetFocalPlane(m->GetFocalPlaneComputedX(), m->GetFocalPlaneComputedY())) {
1209 QString msg =
"Sanity check #1 for ControlPoint [" +
GetId() +
1235 double sample = m->GetSample();
1236 double computedY = m->GetFocalPlaneComputedY();
1237 double computedX = m->GetFocalPlaneComputedX();
1242 adjLine = m->GetLine() - 1.;
1244 adjLine = m->GetLine() + 1.;
1250 cam->
SetImage(sample, m->GetLine());
1259 deltaLine = -computedY/scalingY;
1261 deltaLine = computedY/scalingY;
1272 cuLine = m->GetLine() + deltaLine;
1280 if (!fpmap->
SetFocalPlane(m->GetFocalPlaneMeasuredX(), m->GetFocalPlaneMeasuredY())) {
1281 QString msg =
"Sanity check #2 for ControlPoint [" +
GetId() +
1290 muSamp = m->GetSample();
1291 muLine = m->GetLine();
1297 double sampResidual = muSamp - cuSamp;
1298 double lineResidual = muLine - cuLine;
1332 for (
int j = 0; j < keys.size(); j++) {
1342 Camera *cam = m->Camera();
1353 cam->
SetImage(m->GetSample(), m->GetLine());
1355 cam->
GroundMap()->
GetXY(GetAdjustedSurfacePoint(), &cudx, &cudy);
1368 QString ControlPoint::GetChooserName()
const {
1378 QString ControlPoint::GetDateTime()
const {
1388 bool ControlPoint::IsEditLocked()
const {
1393 bool ControlPoint::IsRejected()
const {
1398 SurfacePoint ControlPoint::GetAdjustedSurfacePoint()
const {
1425 bool ControlPoint::IsIgnored()
const {
1430 bool ControlPoint::IsValid()
const {
1435 bool ControlPoint::IsInvalid()
const {
1450 switch (pointType) {
1455 str =
"Constrained";
1474 QString pointTypeString) {
1479 QString errMsg =
"There is no PointType that has a string representation"
1481 errMsg += pointTypeString;
1484 if (pointTypeString ==
"Fixed")
1486 else if (pointTypeString ==
"Constrained")
1488 else if (pointTypeString ==
"Free")
1527 case RadiusSource::None:
1530 case RadiusSource::User:
1533 case RadiusSource::AverageOfMeasures:
1534 str =
"AverageOfMeasures";
1536 case RadiusSource::Ellipsoid:
1539 case RadiusSource::DEM:
1542 case RadiusSource::BundleSolution:
1543 str =
"BundleSolution";
1561 str = str.toLower();
1562 RadiusSource::Source source = RadiusSource::None;
1565 source = RadiusSource::User;
1566 else if (str ==
"averageofmeasures")
1567 source = RadiusSource::AverageOfMeasures;
1568 else if (str ==
"ellipsoid")
1569 source = RadiusSource::Ellipsoid;
1570 else if (str ==
"dem")
1571 source = RadiusSource::DEM;
1572 else if (str ==
"bundlesolution")
1573 source = RadiusSource::BundleSolution;
1598 SurfacePointSource::Source source) {
1603 case SurfacePointSource::None:
1606 case SurfacePointSource::User:
1609 case SurfacePointSource::AverageOfMeasures:
1610 str =
"AverageOfMeasures";
1612 case SurfacePointSource::Reference:
1615 case SurfacePointSource::Basemap:
1618 case SurfacePointSource::BundleSolution:
1619 str =
"BundleSolution";
1634 ControlPoint::SurfacePointSource::Source
1638 str = str.toLower();
1639 SurfacePointSource::Source source = SurfacePointSource::None;
1642 source = SurfacePointSource::User;
1643 else if (str ==
"averageofmeasures")
1644 source = SurfacePointSource::AverageOfMeasures;
1645 else if (str ==
"reference")
1646 source = SurfacePointSource::Reference;
1647 else if (str ==
"basemap")
1648 source = SurfacePointSource::Basemap;
1649 else if (str ==
"bundlesolution")
1650 source = SurfacePointSource::BundleSolution;
1666 bool ControlPoint::IsFixed()
const {
1671 SurfacePoint ControlPoint::GetAprioriSurfacePoint()
const {
1676 ControlPoint::RadiusSource::Source ControlPoint::GetAprioriRadiusSource()
1681 bool ControlPoint::HasAprioriCoordinates() {
1690 bool ControlPoint::IsConstrained() {
1694 bool ControlPoint::IsLatitudeConstrained() {
1698 bool ControlPoint::IsLongitudeConstrained() {
1702 bool ControlPoint::IsRadiusConstrained() {
1706 int ControlPoint::NumberOfConstrainedCoordinates() {
1710 QString ControlPoint::GetAprioriRadiusSourceFile()
const {
1714 ControlPoint::SurfacePointSource::Source
1715 ControlPoint::GetAprioriSurfacePointSource()
const {
1720 QString ControlPoint::GetAprioriSurfacePointSourceFile()
const {
1725 int ControlPoint::GetNumMeasures()
const {
1726 return measures->size();
1737 for (
int cm = 0; cm < keys.size(); cm++) {
1738 if (!(*measures)[keys[cm]]->IsIgnored())
1753 for (
int cm = 0; cm < keys.size(); cm++) {
1754 if ((*measures)[keys[cm]]->IsEditLocked())
1768 return cubeSerials->contains(serialNumber);
1785 if (referenceMeasure == NULL) {
1786 QString msg =
"There is no reference measure set in the ControlPoint [" +
1818 int index = cubeSerials->indexOf(sn);
1820 if (
throws && index == -1) {
1821 QString msg =
"ControlMeasure [" + sn +
"] does not exist in point [" +
1839 if (!referenceMeasure) {
1840 QString msg =
"There is no reference measure for point [" +
id +
"]."
1841 " This also means of course that the point is empty!";
1867 if (!cm->IsIgnored())
1868 stats.
AddData((cm->*statFunc)());
1878 if (!cm->IsIgnored())
1893 bool excludeIgnored)
const {
1895 for (
int i = 0; i < cubeSerials->size(); i++) {
1897 if (!excludeIgnored || !measure->IsIgnored())
1898 orderedMeasures.append(measures->value((*cubeSerials)[i]));
1900 return orderedMeasures;
1908 return *cubeSerials;
1970 return !(*
this == other);
1983 return other.GetNumMeasures() == GetNumMeasures() &&
1996 other.measures == measures &&
2002 other.cubeSerials == cubeSerials &&
2003 other.referenceMeasure == referenceMeasure;
2025 if (
this != &other) {
2027 for (
int i = cubeSerials->size() - 1; i >= 0; i--) {
2028 (*measures)[cubeSerials->at(i)]->SetEditLock(
false);
2029 Delete(cubeSerials->at(i));
2034 QHashIterator< QString, ControlMeasure * > i(*other.measures);
2035 while (i.hasNext()) {
2038 *newMeasure = *i.value();
2040 if (other.referenceMeasure == i.value())
2067 void ControlPoint::PointModified() {
2108 int nmeasures = measures->size();
2109 if( nmeasures <= 0 )
2114 for(
int i = 0; i < nmeasures; i++) {
2119 if( !m->IsIgnored() || m->IsRejected() )
2122 stats.
AddData(m->GetSampleResidual());
2136 int nmeasures = measures->size();
2137 if( nmeasures <= 0 )
2142 for(
int i = 0; i < nmeasures; i++) {
2147 if( !m->IsIgnored() || m->IsRejected() )
2150 stats.
AddData(m->GetLineResidual());
2164 int nmeasures = measures->size();
2165 if( nmeasures <= 0 )
2170 for(
int i = 0; i < nmeasures; i++) {
2175 if( m->IsIgnored() || m->IsRejected() )
2178 stats.
AddData(m->GetSampleResidual());
2179 stats.
AddData(m->GetLineResidual());
2190 int nmeasures = measures->size();
2191 if( nmeasures <= 0 )
2194 for(
int i = 0; i < nmeasures; i++) {
2209 fileEntry.set_id(
GetId().toAscii().data());
2212 fileEntry.set_type(ControlPointFileEntryV0002::Free);
2215 fileEntry.set_type(ControlPointFileEntryV0002::Constrained);
2218 fileEntry.set_type(ControlPointFileEntryV0002::Fixed);
2222 if (!GetChooserName().isEmpty()) {
2223 fileEntry.set_choosername(GetChooserName().toAscii().data());
2225 if (!GetDateTime().isEmpty()) {
2226 fileEntry.set_datetime(GetDateTime().toAscii().data());
2229 fileEntry.set_editlock(
true);
2231 fileEntry.set_ignore(
true);
2233 fileEntry.set_jigsawrejected(
true);
2239 switch (GetAprioriSurfacePointSource()) {
2240 case ControlPoint::SurfacePointSource::None:
2242 case ControlPoint::SurfacePointSource::User:
2243 fileEntry.set_apriorisurfpointsource(ControlPointFileEntryV0002_AprioriSource_User);
2245 case ControlPoint::SurfacePointSource::AverageOfMeasures:
2246 fileEntry.set_apriorisurfpointsource(ControlPointFileEntryV0002_AprioriSource_AverageOfMeasures);
2248 case ControlPoint::SurfacePointSource::Reference:
2249 fileEntry.set_apriorisurfpointsource(ControlPointFileEntryV0002_AprioriSource_Reference);
2251 case ControlPoint::SurfacePointSource::Basemap:
2252 fileEntry.set_apriorisurfpointsource(ControlPointFileEntryV0002_AprioriSource_Basemap);
2254 case ControlPoint::SurfacePointSource::BundleSolution:
2255 fileEntry.set_apriorisurfpointsource(ControlPointFileEntryV0002_AprioriSource_BundleSolution);
2260 if (!GetAprioriSurfacePointSourceFile().isEmpty()) {
2261 fileEntry.set_apriorisurfpointsourcefile(GetAprioriSurfacePointSourceFile().toAscii().data());
2264 switch (GetAprioriRadiusSource()) {
2265 case ControlPoint::RadiusSource::None:
2267 case ControlPoint::RadiusSource::User:
2268 fileEntry.set_aprioriradiussource(ControlPointFileEntryV0002_AprioriSource_User);
2270 case ControlPoint::RadiusSource::AverageOfMeasures:
2271 fileEntry.set_aprioriradiussource(ControlPointFileEntryV0002_AprioriSource_AverageOfMeasures);
2273 case ControlPoint::RadiusSource::Ellipsoid:
2274 fileEntry.set_aprioriradiussource(ControlPointFileEntryV0002_AprioriSource_Ellipsoid);
2276 case ControlPoint::RadiusSource::DEM:
2277 fileEntry.set_aprioriradiussource(ControlPointFileEntryV0002_AprioriSource_DEM);
2279 case ControlPoint::RadiusSource::BundleSolution:
2280 fileEntry.set_aprioriradiussource(ControlPointFileEntryV0002_AprioriSource_BundleSolution);
2286 if (!GetAprioriRadiusSourceFile().isEmpty()) {
2287 fileEntry.set_aprioriradiussourcefile(GetAprioriRadiusSourceFile().toAscii().data());
2290 if (GetAprioriSurfacePoint().Valid()) {
2291 SurfacePoint apriori = GetAprioriSurfacePoint();
2292 fileEntry.set_apriorix(apriori.GetX().meters());
2293 fileEntry.set_aprioriy(apriori.GetY().meters());
2294 fileEntry.set_aprioriz(apriori.GetZ().meters());
2296 symmetric_matrix< double, upper > covar = apriori.GetRectangularMatrix();
2297 if (covar(0, 0) != 0. || covar(0, 1) != 0. ||
2298 covar(0, 2) != 0. || covar(1, 1) != 0. ||
2299 covar(1, 2) != 0. || covar(2, 2) != 0.) {
2300 fileEntry.add_aprioricovar(covar(0, 0));
2301 fileEntry.add_aprioricovar(covar(0, 1));
2302 fileEntry.add_aprioricovar(covar(0, 2));
2303 fileEntry.add_aprioricovar(covar(1, 1));
2304 fileEntry.add_aprioricovar(covar(1, 2));
2305 fileEntry.add_aprioricovar(covar(2, 2));
2309 fileEntry.set_latitudeconstrained(
true);
2312 fileEntry.set_longitudeconstrained(
true);
2315 fileEntry.set_radiusconstrained(
true);
2319 if (GetAdjustedSurfacePoint().Valid()) {
2320 SurfacePoint adjusted = GetAdjustedSurfacePoint();
2321 fileEntry.set_adjustedx(adjusted.GetX().meters());
2322 fileEntry.set_adjustedy(adjusted.GetY().meters());
2323 fileEntry.set_adjustedz(adjusted.GetZ().meters());
2325 symmetric_matrix< double, upper > covar = adjusted.GetRectangularMatrix();
2326 if (covar(0, 0) != 0. || covar(0, 1) != 0. ||
2327 covar(0, 2) != 0. || covar(1, 1) != 0. ||
2328 covar(1, 2) != 0. || covar(2, 2) != 0.) {
2329 fileEntry.add_adjustedcovar(covar(0, 0));
2330 fileEntry.add_adjustedcovar(covar(0, 1));
2331 fileEntry.add_adjustedcovar(covar(0, 2));
2332 fileEntry.add_adjustedcovar(covar(1, 1));
2333 fileEntry.add_adjustedcovar(covar(1, 2));
2334 fileEntry.add_adjustedcovar(covar(2, 2));
2339 for (
int i = 0; i < cubeSerials->size(); i++) {
2340 *fileEntry.add_measures() =
2341 (*measures)[cubeSerials->at(i)]->ToProtocolBuffer();